Apply ament_uncrustify --reformat (#297)

Signed-off-by: Sean Kelly <sean@seankelly.dev>

pcl_ros/include/pcl_ros/features/boundary.hpp

@@ -66,14 +66,15 @@ namespace pcl_ros
   {
     // Create the output publisher
     pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
-        return (true);
+    return true;
   }
 
   /** \brief Publish an empty point cloud of the feature output type. */
   void emptyPublish(const PointCloudInConstPtr & cloud);
 
   /** \brief Compute the feature and publish it. */
-      void computePublish (const PointCloudInConstPtr &cloud,
+  void computePublish(
+    const PointCloudInConstPtr & cloud,
     const PointCloudNConstPtr & normals,
     const PointCloudInConstPtr & surface,
     const IndicesPtr & indices);
@@ -84,5 +85,3 @@ namespace pcl_ros
 }
 
 #endif  //#ifndef PCL_ROS_BOUNDARY_H_
-
-

pcl_ros/include/pcl_ros/features/feature.hpp

@@ -77,9 +77,10 @@ namespace pcl_ros
   typedef pcl::IndicesConstPtr IndicesConstPtr;
 
   /** \brief Empty constructor. */
-      Feature () : /*input_(), indices_(), surface_(), */tree_(), k_(0), search_radius_(0),
+  Feature()
+  : /*input_(), indices_(), surface_(), */ tree_(), k_(0), search_radius_(0),
     use_surface_(false), spatial_locator_type_(-1)
-      {};
+  {}
 
 protected:
   /** \brief The input point cloud dataset. */
@@ -127,7 +128,8 @@ namespace pcl_ros
   virtual void emptyPublish(const PointCloudInConstPtr & cloud) = 0;
 
   /** \brief Compute the feature and publish it. Internal method. */
-      virtual void computePublish (const PointCloudInConstPtr &cloud, 
+  virtual void computePublish(
+    const PointCloudInConstPtr & cloud,
     const PointCloudInConstPtr & surface,
     const IndicesPtr & indices) = 0;
 
@@ -159,8 +161,10 @@ namespace pcl_ros
 
 private:
   /** \brief Synchronized input, surface, and point indices.*/
-      boost::shared_ptr <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudIn, PointIndices> > > sync_input_surface_indices_a_;
-      boost::shared_ptr <message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudIn, PointIndices> > > sync_input_surface_indices_e_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn,
+    PointCloudIn, PointIndices>>> sync_input_surface_indices_a_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn,
+    PointCloudIn, PointIndices>>> sync_input_surface_indices_e_;
 
   /** \brief Nodelet initialization routine. */
   virtual void onInit();
@@ -174,7 +178,8 @@ namespace pcl_ros
     * \param cloud_surface the pointer to the surface point cloud
     * \param indices the pointer to the input point cloud indices
     */
-      void input_surface_indices_callback (const PointCloudInConstPtr &cloud, 
+  void input_surface_indices_callback(
+    const PointCloudInConstPtr & cloud,
     const PointCloudInConstPtr & cloud_surface,
     const PointIndicesConstPtr & indices);
 
@@ -194,7 +199,8 @@ namespace pcl_ros
   typedef boost::shared_ptr<PointCloudN> PointCloudNPtr;
   typedef boost::shared_ptr<const PointCloudN> PointCloudNConstPtr;
 
-      FeatureFromNormals () : normals_() {};
+  FeatureFromNormals()
+  : normals_() {}
 
 protected:
   /** \brief A pointer to the input dataset that contains the point normals of the XYZ dataset. */
@@ -207,7 +213,8 @@ namespace pcl_ros
   virtual void emptyPublish(const PointCloudInConstPtr & cloud) = 0;
 
   /** \brief Compute the feature and publish it. */
-      virtual void computePublish (const PointCloudInConstPtr &cloud, 
+  virtual void computePublish(
+    const PointCloudInConstPtr & cloud,
     const PointCloudNConstPtr & normals,
     const PointCloudInConstPtr & surface,
     const IndicesPtr & indices) = 0;
@@ -217,11 +224,14 @@ namespace pcl_ros
   message_filters::Subscriber<PointCloudN> sub_normals_filter_;
 
   /** \brief Synchronized input, normals, surface, and point indices.*/
-      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > sync_input_normals_surface_indices_a_;
-      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > sync_input_normals_surface_indices_e_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn,
+    PointCloudN, PointCloudIn, PointIndices>>> sync_input_normals_surface_indices_a_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn,
+    PointCloudN, PointCloudIn, PointIndices>>> sync_input_normals_surface_indices_e_;
 
   /** \brief Internal method. */
-      void computePublish (const PointCloudInConstPtr &, 
+  void computePublish(
+    const PointCloudInConstPtr &,
     const PointCloudInConstPtr &,
     const IndicesPtr &) {}                        // This should never be called
 
@@ -238,7 +248,8 @@ namespace pcl_ros
     * \param cloud_surface the pointer to the surface point cloud
     * \param indices the pointer to the input point cloud indices
     */
-      void input_normals_surface_indices_callback (const PointCloudInConstPtr &cloud, 
+  void input_normals_surface_indices_callback(
+    const PointCloudInConstPtr & cloud,
     const PointCloudNConstPtr & cloud_normals,
     const PointCloudInConstPtr & cloud_surface,
     const PointIndicesConstPtr & indices);

pcl_ros/include/pcl_ros/features/fpfh.hpp

@@ -78,14 +78,15 @@ namespace pcl_ros
   {
     // Create the output publisher
     pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
-        return (true);
+    return true;
   }
 
   /** \brief Publish an empty point cloud of the feature output type. */
   void emptyPublish(const PointCloudInConstPtr & cloud);
 
   /** \brief Compute the feature and publish it. */
-      void computePublish (const PointCloudInConstPtr &cloud,
+  void computePublish(
+    const PointCloudInConstPtr & cloud,
     const PointCloudNConstPtr & normals,
     const PointCloudInConstPtr & surface,
     const IndicesPtr & indices);
@@ -96,5 +97,3 @@ namespace pcl_ros
 }
 
 #endif  //#ifndef PCL_FPFH_H_
-
-

pcl_ros/include/pcl_ros/features/fpfh_omp.hpp

@@ -75,14 +75,15 @@ namespace pcl_ros
   {
     // Create the output publisher
     pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
-        return (true);
+    return true;
   }
 
   /** \brief Publish an empty point cloud of the feature output type. */
   void emptyPublish(const PointCloudInConstPtr & cloud);
 
   /** \brief Compute the feature and publish it. */
-      void computePublish (const PointCloudInConstPtr &cloud,
+  void computePublish(
+    const PointCloudInConstPtr & cloud,
     const PointCloudNConstPtr & normals,
     const PointCloudInConstPtr & surface,
     const IndicesPtr & indices);
@@ -93,4 +94,3 @@ namespace pcl_ros
 }
 
 #endif  //#ifndef PCL_ROS_FPFH_OMP_H_
-

pcl_ros/include/pcl_ros/features/moment_invariants.hpp

@@ -62,14 +62,15 @@ namespace pcl_ros
   {
     // Create the output publisher
     pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
-        return (true);
+    return true;
   }
 
   /** \brief Publish an empty point cloud of the feature output type. */
   void emptyPublish(const PointCloudInConstPtr & cloud);
 
   /** \brief Compute the feature and publish it. */
-      void computePublish (const PointCloudInConstPtr &cloud,
+  void computePublish(
+    const PointCloudInConstPtr & cloud,
     const PointCloudInConstPtr & surface,
     const IndicesPtr & indices);
 
@@ -79,5 +80,3 @@ namespace pcl_ros
 }
 
 #endif  //#ifndef PCL_ROS_MOMENT_INVARIANTS_H_
-
-

pcl_ros/include/pcl_ros/features/normal_3d.hpp

@@ -64,7 +64,7 @@ namespace pcl_ros
   {
     // Create the output publisher
     pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
-        return (true);
+    return true;
   }
 
   /** \brief Publish an empty point cloud of the feature output type.
@@ -73,7 +73,8 @@ namespace pcl_ros
   void emptyPublish(const PointCloudInConstPtr & cloud);
 
   /** \brief Compute the feature and publish it. */
-      void computePublish (const PointCloudInConstPtr &cloud,
+  void computePublish(
+    const PointCloudInConstPtr & cloud,
     const PointCloudInConstPtr & surface,
     const IndicesPtr & indices);
 
@@ -83,4 +84,3 @@ namespace pcl_ros
 }
 
 #endif  //#ifndef PCL_ROS_NORMAL_3D_H_
-

pcl_ros/include/pcl_ros/features/normal_3d_omp.hpp

@@ -60,14 +60,15 @@ namespace pcl_ros
   {
     // Create the output publisher
     pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
-        return (true);
+    return true;
   }
 
   /** \brief Publish an empty point cloud of the feature output type. */
   void emptyPublish(const PointCloudInConstPtr & cloud);
 
   /** \brief Compute the feature and publish it. */
-      void computePublish (const PointCloudInConstPtr &cloud,
+  void computePublish(
+    const PointCloudInConstPtr & cloud,
     const PointCloudInConstPtr & surface,
     const IndicesPtr & indices);
 

pcl_ros/include/pcl_ros/features/normal_3d_tbb.hpp

@@ -63,14 +63,15 @@ namespace pcl_ros
   {
     // Create the output publisher
     pub_output_ = advertise<PointCloud>(nh, "output", max_queue_size_);
-        return (true);
+    return true;
   }
 
   /** \brief Publish an empty point cloud of the feature output type. */
   void emptyPublish(const PointCloudInConstPtr & cloud);
 
   /** \brief Compute the feature and publish it. */
-      void computePublish (const PointCloudInConstPtr &cloud,
+  void computePublish(
+    const PointCloudInConstPtr & cloud,
     const PointCloudInConstPtr & surface,
     const IndicesPtr & indices);
 
@@ -82,5 +83,3 @@ namespace pcl_ros
 //#endif  // HAVE_TBB
 
 #endif  //#ifndef PCL_ROS_NORMAL_3D_TBB_H_
-
-

pcl_ros/include/pcl_ros/features/pfh.hpp

@@ -78,14 +78,15 @@ namespace pcl_ros
   {
     // Create the output publisher
     pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
-        return (true);
+    return true;
   }
 
   /** \brief Publish an empty point cloud of the feature output type. */
   void emptyPublish(const PointCloudInConstPtr & cloud);
 
   /** \brief Compute the feature and publish it. */
-      void computePublish (const PointCloudInConstPtr &cloud,
+  void computePublish(
+    const PointCloudInConstPtr & cloud,
     const PointCloudNConstPtr & normals,
     const PointCloudInConstPtr & surface,
     const IndicesPtr & indices);
@@ -96,5 +97,3 @@ namespace pcl_ros
 }
 
 #endif  //#ifndef PCL_ROS_PFH_H_
-
-

pcl_ros/include/pcl_ros/features/principal_curvatures.hpp

@@ -64,14 +64,15 @@ namespace pcl_ros
   {
     // Create the output publisher
     pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
-        return (true);
+    return true;
   }
 
   /** \brief Publish an empty point cloud of the feature output type. */
   void emptyPublish(const PointCloudInConstPtr & cloud);
 
   /** \brief Compute the feature and publish it. */
-      void computePublish (const PointCloudInConstPtr &cloud,
+  void computePublish(
+    const PointCloudInConstPtr & cloud,
     const PointCloudNConstPtr & normals,
     const PointCloudInConstPtr & surface,
     const IndicesPtr & indices);

pcl_ros/include/pcl_ros/features/shot.hpp

@@ -58,14 +58,15 @@ namespace pcl_ros
   {
     // Create the output publisher
     pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
-        return (true);
+    return true;
   }
 
   /** \brief Publish an empty point cloud of the feature output type. */
   void emptyPublish(const PointCloudInConstPtr & cloud);
 
   /** \brief Compute the feature and publish it. */
-      void computePublish (const PointCloudInConstPtr &cloud,
+  void computePublish(
+    const PointCloudInConstPtr & cloud,
     const PointCloudNConstPtr & normals,
     const PointCloudInConstPtr & surface,
     const IndicesPtr & indices);
@@ -76,5 +77,3 @@ namespace pcl_ros
 }
 
 #endif  //#ifndef PCL_SHOT_H_
-
-

pcl_ros/include/pcl_ros/features/shot_omp.hpp

@@ -57,14 +57,15 @@ namespace pcl_ros
   {
     // Create the output publisher
     pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
-        return (true);
+    return true;
   }
 
   /** \brief Publish an empty point cloud of the feature output type. */
   void emptyPublish(const PointCloudInConstPtr & cloud);
 
   /** \brief Compute the feature and publish it. */
-      void computePublish (const PointCloudInConstPtr &cloud,
+  void computePublish(
+    const PointCloudInConstPtr & cloud,
     const PointCloudNConstPtr & normals,
     const PointCloudInConstPtr & surface,
     const IndicesPtr & indices);
@@ -75,4 +76,3 @@ namespace pcl_ros
 }
 
 #endif  //#ifndef PCL_ROS_SHOT_OMP_H_
-

pcl_ros/include/pcl_ros/features/vfh.hpp

@@ -63,14 +63,15 @@ namespace pcl_ros
   {
     // Create the output publisher
     pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
-        return (true);
+    return true;
   }
 
   /** \brief Publish an empty point cloud of the feature output type. */
   void emptyPublish(const PointCloudInConstPtr & cloud);
 
   /** \brief Compute the feature and publish it. */
-      void computePublish (const PointCloudInConstPtr &cloud,
+  void computePublish(
+    const PointCloudInConstPtr & cloud,
     const PointCloudNConstPtr & normals,
     const PointCloudInConstPtr & surface,
     const IndicesPtr & indices);

pcl_ros/include/pcl_ros/filters/crop_box.hpp

@@ -66,7 +66,8 @@ namespace pcl_ros
     * \param output the resultant filtered dataset
     */
   inline void
-      filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, 
+  filter(
+    const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
     PointCloud2 & output)
   {
     boost::mutex::scoped_lock lock(mutex_);
@@ -96,6 +97,7 @@ namespace pcl_ros
 private:
   /** \brief The PCL filter implementation used. */
   pcl::CropBox<pcl::PCLPointCloud2> impl_;
+
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };

pcl_ros/include/pcl_ros/filters/extract_indices.hpp

@@ -62,7 +62,8 @@ namespace pcl_ros
     * \param output the resultant filtered dataset
     */
   inline void
-      filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, 
+  filter(
+    const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
     PointCloud2 & output)
   {
     boost::mutex::scoped_lock lock(mutex_);
@@ -89,10 +90,10 @@ namespace pcl_ros
 private:
   /** \brief The PCL filter implementation used. */
   pcl::ExtractIndices<pcl::PCLPointCloud2> impl_;
+
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 }
 
 #endif  //#ifndef PCL_ROS_FILTERS_EXTRACTINDICES_H_
-

pcl_ros/include/pcl_ros/filters/filter.hpp

@@ -102,7 +102,7 @@ namespace pcl_ros
   child_init(ros::NodeHandle & nh, bool & has_service)
   {
     has_service = false;
-        return (true); 
+    return true;
   }
 
   /** \brief Virtual abstract filter method. To be implemented by every child.
@@ -111,7 +111,8 @@ namespace pcl_ros
     * \param output the resultant filtered PointCloud2
     */
   virtual void
-      filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, 
+  filter(
+    const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
     PointCloud2 & output) = 0;
 
   /** \brief Lazy transport subscribe routine. */
@@ -138,8 +139,10 @@ namespace pcl_ros
   boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::FilterConfig>> srv_;
 
   /** \brief Synchronized input, and indices.*/
-      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointIndices> > >       sync_input_indices_e_;
-      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, PointIndices> > > sync_input_indices_a_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2,
+    PointIndices>>> sync_input_indices_e_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2,
+    PointIndices>>> sync_input_indices_a_;
 
   /** \brief Dynamic reconfigure service callback. */
   virtual void
@@ -147,8 +150,10 @@ namespace pcl_ros
 
   /** \brief PointCloud2 + Indices data callback. */
   void
-      input_indices_callback (const PointCloud2::ConstPtr &cloud, 
+  input_indices_callback(
+    const PointCloud2::ConstPtr & cloud,
     const PointIndicesConstPtr & indices);
+
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };

pcl_ros/include/pcl_ros/filters/passthrough.hpp

@@ -60,7 +60,8 @@ namespace pcl_ros
     * \param output the resultant filtered dataset
     */
   inline void
-      filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, 
+  filter(
+    const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
     PointCloud2 & output)
   {
     boost::mutex::scoped_lock lock(mutex_);
@@ -90,6 +91,7 @@ namespace pcl_ros
 private:
   /** \brief The PCL filter implementation used. */
   pcl::PassThrough<pcl::PCLPointCloud2> impl_;
+
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };

pcl_ros/include/pcl_ros/filters/project_inliers.hpp

@@ -56,7 +56,8 @@ namespace pcl_ros
 class ProjectInliers : public Filter
 {
 public:
-      ProjectInliers () : model_ () {}
+  ProjectInliers()
+  : model_() {}
 
 protected:
   /** \brief Call the actual filter.
@@ -65,7 +66,8 @@ namespace pcl_ros
     * \param output the resultant filtered dataset
     */
   inline void
-      filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, 
+  filter(
+    const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
     PointCloud2 & output)
   {
     pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
@@ -88,8 +90,10 @@ namespace pcl_ros
   message_filters::Subscriber<ModelCoefficients> sub_model_;
 
   /** \brief Synchronized input, indices, and model coefficients.*/
-      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointIndices, ModelCoefficients> > > sync_input_indices_model_e_;
-      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, PointIndices, ModelCoefficients> > > sync_input_indices_model_a_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2,
+    PointIndices, ModelCoefficients>>> sync_input_indices_model_e_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2,
+    PointIndices, ModelCoefficients>>> sync_input_indices_model_a_;
   /** \brief The PCL filter implementation used. */
   pcl::ProjectInliers<pcl::PCLPointCloud2> impl_;
 
@@ -103,9 +107,11 @@ namespace pcl_ros
 
   /** \brief PointCloud2 + Indices + Model data callback. */
   void
-      input_indices_model_callback (const PointCloud2::ConstPtr &cloud, 
+  input_indices_model_callback(
+    const PointCloud2::ConstPtr & cloud,
     const PointIndicesConstPtr & indices,
     const ModelCoefficientsConstPtr & model);
+
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };

pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp

@@ -64,7 +64,8 @@ namespace pcl_ros
     * \param output the resultant filtered dataset
     */
   inline void
-      filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, 
+  filter(
+    const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
     PointCloud2 & output)
   {
     pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
@@ -88,10 +89,10 @@ namespace pcl_ros
     */
   void config_callback(pcl_ros::RadiusOutlierRemovalConfig & config, uint32_t level);
 
-    
 private:
   /** \brief The PCL filter implementation used. */
   pcl::RadiusOutlierRemoval<pcl::PCLPointCloud2> impl_;
+
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };

pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp

@@ -70,7 +70,8 @@ namespace pcl_ros
     * \param output the resultant filtered dataset
     */
   inline void
-      filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, 
+  filter(
+    const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
     PointCloud2 & output)
   {
     boost::mutex::scoped_lock lock(mutex_);
@@ -98,6 +99,7 @@ namespace pcl_ros
 private:
   /** \brief The PCL filter implementation used. */
   pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2> impl_;
+
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };

pcl_ros/include/pcl_ros/filters/voxel_grid.hpp

@@ -65,7 +65,8 @@ namespace pcl_ros
     * \param output the resultant filtered dataset
     */
   virtual void
-      filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, 
+  filter(
+    const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
     PointCloud2 & output);
 
   /** \brief Child initialization routine.
@@ -81,6 +82,7 @@ namespace pcl_ros
     */
   void
   config_callback(pcl_ros::VoxelGridConfig & config, uint32_t level);
+
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };

pcl_ros/include/pcl_ros/impl/transforms.hpp

@@ -46,8 +46,10 @@ using pcl_conversions::toPCL;
 namespace pcl_ros
 {
 //////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT> void
-transformPointCloudWithNormals (const pcl::PointCloud <PointT> &cloud_in,
+template<typename PointT>
+void
+transformPointCloudWithNormals(
+  const pcl::PointCloud<PointT> & cloud_in,
   pcl::PointCloud<PointT> & cloud_out, const tf::Transform & transform)
 {
   // Bullet (used by tf) and Eigen both store quaternions in x,y,z,w order, despite the ordering
@@ -67,8 +69,10 @@ transformPointCloudWithNormals (const pcl::PointCloud <PointT> &cloud_in,
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT> void
-transformPointCloud (const pcl::PointCloud <PointT> &cloud_in,
+template<typename PointT>
+void
+transformPointCloud(
+  const pcl::PointCloud<PointT> & cloud_in,
   pcl::PointCloud<PointT> & cloud_out, const tf::Transform & transform)
 {
   // Bullet (used by tf) and Eigen both store quaternions in x,y,z,w order, despite the ordering
@@ -88,75 +92,73 @@ transformPointCloud (const pcl::PointCloud <PointT> &cloud_in,
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT> bool
-transformPointCloudWithNormals (const std::string &target_frame,
+template<typename PointT>
+bool
+transformPointCloudWithNormals(
+  const std::string & target_frame,
   const pcl::PointCloud<PointT> & cloud_in,
   pcl::PointCloud<PointT> & cloud_out,
   const tf::TransformListener & tf_listener)
 {
-  if (cloud_in.header.frame_id == target_frame)
-  {
+  if (cloud_in.header.frame_id == target_frame) {
     cloud_out = cloud_in;
-    return (true);
+    return true;
   }
 
   tf::StampedTransform transform;
-  try
-  {
-    tf_listener.lookupTransform (target_frame, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, transform);
-  }
-  catch (tf::LookupException &e)
-  {
+  try {
+    tf_listener.lookupTransform(
+      target_frame, cloud_in.header.frame_id, fromPCL(
+        cloud_in.header).stamp, transform);
+  } catch (tf::LookupException & e) {
     ROS_ERROR("%s", e.what());
-    return (false);
-  }
-  catch (tf::ExtrapolationException &e)
-  {
+    return false;
+  } catch (tf::ExtrapolationException & e) {
     ROS_ERROR("%s", e.what());
-    return (false);
+    return false;
   }
 
   transformPointCloudWithNormals(cloud_in, cloud_out, transform);
   cloud_out.header.frame_id = target_frame;
-  return (true);
+  return true;
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT> bool
-transformPointCloud (const std::string &target_frame,
+template<typename PointT>
+bool
+transformPointCloud(
+  const std::string & target_frame,
   const pcl::PointCloud<PointT> & cloud_in,
   pcl::PointCloud<PointT> & cloud_out,
   const tf::TransformListener & tf_listener)
 {
-  if (cloud_in.header.frame_id == target_frame)
-  {
+  if (cloud_in.header.frame_id == target_frame) {
     cloud_out = cloud_in;
-    return (true);
+    return true;
   }
 
   tf::StampedTransform transform;
-  try
-  {
-    tf_listener.lookupTransform (target_frame, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, transform);
-  }
-  catch (tf::LookupException &e)
-  {
+  try {
+    tf_listener.lookupTransform(
+      target_frame, cloud_in.header.frame_id, fromPCL(
+        cloud_in.header).stamp, transform);
+  } catch (tf::LookupException & e) {
     ROS_ERROR("%s", e.what());
-    return (false);
-  }
-  catch (tf::ExtrapolationException &e)
-  {
+    return false;
+  } catch (tf::ExtrapolationException & e) {
     ROS_ERROR("%s", e.what());
-    return (false);
+    return false;
   }
   transformPointCloud(cloud_in, cloud_out, transform);
   cloud_out.header.frame_id = target_frame;
-  return (true);
+  return true;
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT> bool
-transformPointCloud (const std::string &target_frame,
+template<typename PointT>
+bool
+transformPointCloud(
+  const std::string & target_frame,
   const ros::Time & target_time,
   const pcl::PointCloud<PointT> & cloud_in,
   const std::string & fixed_frame,
@@ -164,19 +166,16 @@ transformPointCloud (const std::string &target_frame,
   const tf::TransformListener & tf_listener)
 {
   tf::StampedTransform transform;
-  try
-  {
-    tf_listener.lookupTransform (target_frame, target_time, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, fixed_frame, transform);
-  }
-  catch (tf::LookupException &e)
-  {
+  try {
+    tf_listener.lookupTransform(
+      target_frame, target_time, cloud_in.header.frame_id,
+      fromPCL(cloud_in.header).stamp, fixed_frame, transform);
+  } catch (tf::LookupException & e) {
     ROS_ERROR("%s", e.what());
-    return (false);
-  }
-  catch (tf::ExtrapolationException &e)
-  {
+    return false;
+  } catch (tf::ExtrapolationException & e) {
     ROS_ERROR("%s", e.what());
-    return (false);
+    return false;
   }
 
   transformPointCloud(cloud_in, cloud_out, transform);
@@ -184,12 +183,14 @@ transformPointCloud (const std::string &target_frame,
   std_msgs::Header header;
   header.stamp = target_time;
   cloud_out.header = toPCL(header);
-  return (true);
+  return true;
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT> bool
-transformPointCloudWithNormals (const std::string &target_frame,
+template<typename PointT>
+bool
+transformPointCloudWithNormals(
+  const std::string & target_frame,
   const ros::Time & target_time,
   const pcl::PointCloud<PointT> & cloud_in,
   const std::string & fixed_frame,
@@ -197,19 +198,16 @@ transformPointCloudWithNormals (const std::string &target_frame,
   const tf::TransformListener & tf_listener)
 {
   tf::StampedTransform transform;
-  try
-  {
-    tf_listener.lookupTransform (target_frame, target_time, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, fixed_frame, transform);
-  }
-  catch (tf::LookupException &e)
-  {
+  try {
+    tf_listener.lookupTransform(
+      target_frame, target_time, cloud_in.header.frame_id,
+      fromPCL(cloud_in.header).stamp, fixed_frame, transform);
+  } catch (tf::LookupException & e) {
     ROS_ERROR("%s", e.what());
-    return (false);
-  }
-  catch (tf::ExtrapolationException &e)
-  {
+    return false;
+  } catch (tf::ExtrapolationException & e) {
     ROS_ERROR("%s", e.what());
-    return (false);
+    return false;
   }
 
   transformPointCloudWithNormals(cloud_in, cloud_out, transform);
@@ -217,7 +215,7 @@ transformPointCloudWithNormals (const std::string &target_frame,
   std_msgs::Header header;
   header.stamp = target_time;
   cloud_out.header = toPCL(header);
-  return (true);
+  return true;
 }
 
 }  // namespace pcl_ros

pcl_ros/include/pcl_ros/io/bag_io.hpp

@@ -57,7 +57,8 @@ namespace pcl_ros
   typedef PointCloud::ConstPtr PointCloudConstPtr;
 
   /** \brief Empty constructor. */
-      BAGReader () : publish_rate_ (0), output_ ()/*, cloud_received_ (false)*/ {};
+  BAGReader()
+  : publish_rate_(0), output_() /*, cloud_received_ (false)*/ {}
 
   /** \brief Set the publishing rate in seconds.
     * \param publish_rate the publishing rate in seconds
@@ -65,7 +66,7 @@ namespace pcl_ros
   inline void setPublishRate(double publish_rate) {publish_rate_ = publish_rate;}
 
   /** \brief Get the publishing rate in seconds. */
-      inline double getPublishRate () { return (publish_rate_); }
+  inline double getPublishRate() {return publish_rate_;}
 
   /** \brief Get the next point cloud dataset in the BAG file.
     * \return the next point cloud dataset as read from the file
@@ -73,12 +74,11 @@ namespace pcl_ros
   inline PointCloudConstPtr
   getNextCloud()
   {
-        if (it_ != view_.end ())
-        {
+    if (it_ != view_.end()) {
       output_ = it_->instantiate<sensor_msgs::PointCloud2>();
       ++it_;
     }
-        return (output_);
+    return output_;
   }
 
   /** \brief Open a BAG file for reading and select a specified topic
@@ -121,6 +121,7 @@ namespace pcl_ros
 
   /** \brief Signals that a new PointCloud2 message has been read from the file. */
   //bool cloud_received_;
+
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };

pcl_ros/include/pcl_ros/io/concatenate_data.hpp

@@ -65,15 +65,17 @@ namespace pcl_ros
   typedef PointCloud2::ConstPtr PointCloud2ConstPtr;
 
   /** \brief Empty constructor. */
-      PointCloudConcatenateDataSynchronizer () : maximum_queue_size_ (3) {};
+  PointCloudConcatenateDataSynchronizer()
+  : maximum_queue_size_(3) {}
 
   /** \brief Empty constructor.
     * \param queue_size the maximum queue size
     */
-      PointCloudConcatenateDataSynchronizer (int queue_size) : maximum_queue_size_(queue_size), approximate_sync_(false) {};
+  PointCloudConcatenateDataSynchronizer(int queue_size)
+  : maximum_queue_size_(queue_size), approximate_sync_(false) {}
 
   /** \brief Empty destructor. */
-      virtual ~PointCloudConcatenateDataSynchronizer () {};
+  virtual ~PointCloudConcatenateDataSynchronizer() {}
 
   void onInit();
   void subscribe();
@@ -108,8 +110,11 @@ namespace pcl_ros
   /** \brief Synchronizer.
     * \note This will most likely be rewritten soon using the DynamicTimeSynchronizer.
     */
-      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2> > > ts_a_;
-      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2> > > ts_e_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2,
+    PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2,
+    PointCloud2>>> ts_a_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointCloud2,
+    PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2>>> ts_e_;
 
   /** \brief Input point cloud callback.
     * Because we want to use the same synchronizer object, we push back
@@ -124,7 +129,8 @@ namespace pcl_ros
   }
 
   /** \brief Input callback for 8 synchronized topics. */
-      void input (const PointCloud2::ConstPtr &in1, const PointCloud2::ConstPtr &in2, 
+  void input(
+    const PointCloud2::ConstPtr & in1, const PointCloud2::ConstPtr & in2,
     const PointCloud2::ConstPtr & in3, const PointCloud2::ConstPtr & in4,
     const PointCloud2::ConstPtr & in5, const PointCloud2::ConstPtr & in6,
     const PointCloud2::ConstPtr & in7, const PointCloud2::ConstPtr & in8);

pcl_ros/include/pcl_ros/io/concatenate_fields.hpp

@@ -62,15 +62,17 @@ namespace pcl_ros
   typedef PointCloud::ConstPtr PointCloudConstPtr;
 
   /** \brief Empty constructor. */
-      PointCloudConcatenateFieldsSynchronizer () : maximum_queue_size_ (3), maximum_seconds_ (0) {};
+  PointCloudConcatenateFieldsSynchronizer()
+  : maximum_queue_size_(3), maximum_seconds_(0) {}
 
   /** \brief Empty constructor.
     * \param queue_size the maximum queue size
     */
-      PointCloudConcatenateFieldsSynchronizer (int queue_size) : maximum_queue_size_ (queue_size), maximum_seconds_ (0) {};
+  PointCloudConcatenateFieldsSynchronizer(int queue_size)
+  : maximum_queue_size_(queue_size), maximum_seconds_(0) {}
 
   /** \brief Empty destructor. */
-      virtual ~PointCloudConcatenateFieldsSynchronizer () {};
+  virtual ~PointCloudConcatenateFieldsSynchronizer() {}
 
   void onInit();
   void subscribe();

pcl_ros/include/pcl_ros/io/pcd_io.hpp

@@ -55,7 +55,8 @@ namespace pcl_ros
   typedef PointCloud2::ConstPtr PointCloud2ConstPtr;
 
   /** \brief Empty constructor. */
-      PCDReader () : publish_rate_ (0), tf_frame_ ("/base_link") {};
+  PCDReader()
+  : publish_rate_(0), tf_frame_("/base_link") {}
 
   virtual void onInit();
 
@@ -65,7 +66,7 @@ namespace pcl_ros
   inline void setPublishRate(double publish_rate) {publish_rate_ = publish_rate;}
 
   /** \brief Get the publishing rate in seconds. */
-      inline double getPublishRate () { return (publish_rate_); }
+  inline double getPublishRate() {return publish_rate_;}
 
   /** \brief Set the TF frame the PointCloud will be published in.
     * \param tf_frame the TF frame the PointCloud will be published in
@@ -73,7 +74,7 @@ namespace pcl_ros
   inline void setTFframe(std::string tf_frame) {tf_frame_ = tf_frame;}
 
   /** \brief Get the TF frame the PointCloud is published in. */
-      inline std::string getTFframe () { return (tf_frame_); }
+  inline std::string getTFframe() {return tf_frame_;}
 
 protected:
   /** \brief The publishing interval in seconds. Set to 0 to only publish once (default). */
@@ -91,6 +92,7 @@ namespace pcl_ros
 private:
   /** \brief The PCL implementation used. */
   pcl::PCDReader impl_;
+
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
@@ -102,7 +104,8 @@ namespace pcl_ros
 class PCDWriter : public PCLNodelet
 {
 public:
-      PCDWriter () : file_name_ (""), binary_mode_ (true) {}
+  PCDWriter()
+  : file_name_(""), binary_mode_(true) {}
 
   typedef sensor_msgs::PointCloud2 PointCloud2;
   typedef PointCloud2::Ptr PointCloud2Ptr;
@@ -124,6 +127,7 @@ namespace pcl_ros
 private:
   /** \brief The PCL implementation used. */
   pcl::PCDWriter impl_;
+
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };

pcl_ros/include/pcl_ros/pcl_nodelet.hpp

@@ -91,8 +91,9 @@ namespace pcl_ros
   typedef pcl::IndicesConstPtr IndicesConstPtr;
 
   /** \brief Empty constructor. */
-      PCLNodelet () : use_indices_ (false), latched_indices_ (false),
-                      max_queue_size_ (3), approximate_sync_ (false) {};
+  PCLNodelet()
+  : use_indices_(false), latched_indices_(false),
+    max_queue_size_(3), approximate_sync_(false) {}
 
 protected:
   /** \brief Set to true if point indices are used.
@@ -140,13 +141,17 @@ namespace pcl_ros
   inline bool
   isValid(const PointCloud2::ConstPtr & cloud, const std::string & topic_name = "input")
   {
-        if (cloud->width * cloud->height * cloud->point_step != cloud->data.size ())
-        {
-          NODELET_WARN ("[%s] Invalid PointCloud (data = %zu, width = %d, height = %d, step = %d) with stamp %f, and frame %s on topic %s received!", getName ().c_str (), cloud->data.size (), cloud->width, cloud->height, cloud->point_step, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ());
+    if (cloud->width * cloud->height * cloud->point_step != cloud->data.size()) {
+      NODELET_WARN(
+        "[%s] Invalid PointCloud (data = %zu, width = %d, height = %d, step = %d) with stamp %f, and frame %s on topic %s received!",
+        getName().c_str(),
+        cloud->data.size(), cloud->width, cloud->height, cloud->point_step,
+        cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
+          topic_name).c_str());
 
-          return (false);
+      return false;
     }
-        return (true);
+    return true;
   }
 
   /** \brief Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-zero).
@@ -156,13 +161,16 @@ namespace pcl_ros
   inline bool
   isValid(const PointCloudConstPtr & cloud, const std::string & topic_name = "input")
   {
-        if (cloud->width * cloud->height != cloud->points.size ())
-        {
-          NODELET_WARN ("[%s] Invalid PointCloud (points = %zu, width = %d, height = %d) with stamp %f, and frame %s on topic %s received!", getName ().c_str (), cloud->points.size (), cloud->width, cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ());
+    if (cloud->width * cloud->height != cloud->points.size()) {
+      NODELET_WARN(
+        "[%s] Invalid PointCloud (points = %zu, width = %d, height = %d) with stamp %f, and frame %s on topic %s received!",
+        getName().c_str(), cloud->points.size(), cloud->width, cloud->height,
+        fromPCL(cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(),
+        pnh_->resolveName(topic_name).c_str());
 
-          return (false);
+      return false;
     }
-        return (true);
+    return true;
   }
 
   /** \brief Test whether a given PointIndices message is "valid" (i.e., has values).
@@ -177,7 +185,7 @@ namespace pcl_ros
       NODELET_WARN ("[%s] Empty indices (values = %zu) with stamp %f, and frame %s on topic %s received!", getName ().c_str (), indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ());
       return (true);
     }*/
-        return (true);
+    return true;
   }
 
   /** \brief Test whether a given ModelCoefficients message is "valid" (i.e., has values).
@@ -192,7 +200,7 @@ namespace pcl_ros
       NODELET_WARN ("[%s] Empty model (values = %zu) with stamp %f, and frame %s on topic %s received!", getName ().c_str (), model->values.size (), model->header.stamp.toSec (), model->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ());
       return (false);
     }*/
-        return (true);
+    return true;
   }
 
   /** \brief Lazy transport subscribe/unsubscribe routine. It is optional for backward compatibility. */
@@ -213,7 +221,8 @@ namespace pcl_ros
     pnh_->getParam("latched_indices", latched_indices_);
     pnh_->getParam("approximate_sync", approximate_sync_);
 
-        NODELET_DEBUG ("[%s::onInit] PCL Nodelet successfully created with the following parameters:\n"
+    NODELET_DEBUG(
+      "[%s::onInit] PCL Nodelet successfully created with the following parameters:\n"
       " - approximate_sync : %s\n"
       " - use_indices      : %s\n"
       " - latched_indices  : %s\n"

pcl_ros/include/pcl_ros/point_cloud.hpp

@@ -18,15 +18,18 @@ namespace pcl
 template<typename Stream, typename PointT>
 struct FieldStreamer
 {
-      FieldStreamer(Stream& stream) : stream_(stream) {}
+  FieldStreamer(Stream & stream)
+  : stream_(stream) {}
 
-      template<typename U> void operator() ()
+  template<typename U>
+  void operator()()
   {
     const char * name = traits::name<PointT, U>::value;
     std::uint32_t name_length = strlen(name);
     stream_.next(name_length);
-        if (name_length > 0)
+    if (name_length > 0) {
       memcpy(stream_.advance(name_length), name, name_length);
+    }
 
     std::uint32_t offset = traits::offset<PointT, U>::value;
     stream_.next(offset);
@@ -44,9 +47,11 @@ namespace pcl
 template<typename PointT>
 struct FieldsLength
 {
-      FieldsLength() : length(0) {}
+  FieldsLength()
+  : length(0) {}
 
-      template<typename U> void operator() ()
+  template<typename U>
+  void operator()()
   {
     std::uint32_t name_length = strlen(traits::name<PointT, U>::value);
     length += name_length + 13;
@@ -84,7 +89,8 @@ namespace ros
 
 namespace message_traits
 {
-    template<typename T> struct MD5Sum<pcl::PointCloud<T> >
+template<typename T>
+struct MD5Sum<pcl::PointCloud<T>>
 {
   static const char * value() {return MD5Sum<sensor_msgs::PointCloud2>::value();}
   static const char * value(const pcl::PointCloud<T> &) {return value();}
@@ -97,13 +103,15 @@ namespace ros
   ROS_STATIC_ASSERT(static_value2 == 0xce2f1be655c3c181ULL);
 };
 
-    template<typename T> struct DataType<pcl::PointCloud<T> >
+template<typename T>
+struct DataType<pcl::PointCloud<T>>
 {
   static const char * value() {return DataType<sensor_msgs::PointCloud2>::value();}
   static const char * value(const pcl::PointCloud<T> &) {return value();}
 };
 
-    template<typename T> struct Definition<pcl::PointCloud<T> >
+template<typename T>
+struct Definition<pcl::PointCloud<T>>
 {
   static const char * value() {return Definition<sensor_msgs::PointCloud2>::value();}
   static const char * value(const pcl::PointCloud<T> &) {return value();}
@@ -112,7 +120,8 @@ namespace ros
 // pcl point clouds message don't have a ROS compatible header
 // the specialized meta functions below (TimeStamp and FrameId)
 // can be used to get the header data.
-    template<typename T> struct HasHeader<pcl::PointCloud<T> > : FalseType {};
+template<typename T>
+struct HasHeader<pcl::PointCloud<T>>: FalseType {};
 
 template<typename T>
 struct TimeStamp<pcl::PointCloud<T>>
@@ -122,19 +131,23 @@ namespace ros
   // pointer returned by the first functions may go out of scope, but there
   // isn't a lot I can do about that. This is a good reason to refuse to
   // returning pointers like this...
-      static ros::Time* pointer(typename pcl::PointCloud<T> &m) {
+  static ros::Time * pointer(typename pcl::PointCloud<T> & m)
+  {
     header_.reset(new std_msgs::Header());
     pcl_conversions::fromPCL(m.header, *(header_));
     return &(header_->stamp);
   }
-      static ros::Time const* pointer(const typename pcl::PointCloud<T>& m) {
+  static ros::Time const * pointer(const typename pcl::PointCloud<T> & m)
+  {
     header_const_.reset(new std_msgs::Header());
     pcl_conversions::fromPCL(m.header, *(header_const_));
     return &(header_const_->stamp);
   }
-      static ros::Time value(const typename pcl::PointCloud<T>& m) {
+  static ros::Time value(const typename pcl::PointCloud<T> & m)
+  {
     return pcl_conversions::fromPCL(m.header).stamp;
   }
+
 private:
   static boost::shared_ptr<std_msgs::Header> header_;
   static boost::shared_ptr<std_msgs::Header> header_const_;
@@ -207,14 +220,14 @@ namespace ros
 
     // Construct field mapping if deserializing for the first time
     boost::shared_ptr<pcl::MsgFieldMap> & mapping_ptr = pcl::detail::getMapping(m);
-        if (!mapping_ptr)
-        {
+    if (!mapping_ptr) {
       // This normally should get allocated by DefaultMessageCreator, but just in case
       mapping_ptr = boost::make_shared<pcl::MsgFieldMap>();
     }
     pcl::MsgFieldMap & mapping = *mapping_ptr;
-        if (mapping.empty())
+    if (mapping.empty()) {
       pcl::createMapping<T>(fields, mapping);
+    }
 
     uint8_t is_bigendian;
     stream.next(is_bigendian);     // ignoring...
@@ -236,18 +249,14 @@ namespace ros
     {
       uint32_t m_row_step = sizeof(T) * m.width;
       // And if the row steps match, can copy whole point cloud in one memcpy
-          if (m_row_step == row_step)
-          {
+      if (m_row_step == row_step) {
         memcpy(m_data, stream.advance(data_size), data_size);
-          }
-          else
-          {
-            for (uint32_t i = 0; i < m.height; ++i, m_data += m_row_step)
+      } else {
+        for (uint32_t i = 0; i < m.height; ++i, m_data += m_row_step) {
           memcpy(m_data, stream.advance(row_step), m_row_step);
         }
       }
-        else
-        {
+    } else {
       // If not, do a lot of memcpys to copy over the fields
       for (uint32_t row = 0; row < m.height; ++row) {
         const uint8_t * stream_data = stream.advance(row_step);
@@ -327,13 +336,17 @@ namespace pcl
 constexpr static bool pcl_uses_boost = true;
 #endif
 
-    template<class SharedPointer> struct Holder
+template<class SharedPointer>
+struct Holder
 {
   SharedPointer p;
 
-      Holder(const SharedPointer &p) : p(p) {}
-      Holder(const Holder &other) : p(other.p) {}
-      Holder(Holder &&other) : p(std::move(other.p)) {}
+  Holder(const SharedPointer & p)
+  : p(p) {}
+  Holder(const Holder & other)
+  : p(other.p) {}
+  Holder(Holder && other)
+  : p(std::move(other.p)) {}
 
   void operator()(...) {p.reset();}
 };
@@ -342,12 +355,9 @@ namespace pcl
 inline std::shared_ptr<T> to_std_ptr(const boost::shared_ptr<T> & p)
 {
   typedef Holder<std::shared_ptr<T>> H;
-        if(H *h = boost::get_deleter<H>(p))
-        {
+  if (H * h = boost::get_deleter<H>(p)) {
     return h->p;
-        }
-        else
-        {
+  } else {
     return std::shared_ptr<T>(p.get(), Holder<boost::shared_ptr<T>>(p));
   }
 }
@@ -356,12 +366,9 @@ namespace pcl
 inline boost::shared_ptr<T> to_boost_ptr(const std::shared_ptr<T> & p)
 {
   typedef Holder<boost::shared_ptr<T>> H;
-        if(H * h = std::get_deleter<H>(p))
-        {
+  if (H * h = std::get_deleter<H>(p)) {
     return h->p;
-        }
-        else
-        {
+  } else {
     return boost::shared_ptr<T>(p.get(), Holder<std::shared_ptr<T>>(p));
   }
 }

pcl_ros/include/pcl_ros/publisher.hpp

@@ -65,13 +65,13 @@ namespace pcl_ros
   std::string
   getTopic()
   {
-        return (pub_.getTopic ());
+    return pub_.getTopic();
   }
 
   uint32_t
   getNumSubscribers() const
   {
-        return (pub_.getNumSubscribers ());
+    return pub_.getNumSubscribers();
   }
 
   void

pcl_ros/include/pcl_ros/segmentation/extract_clusters.hpp

@@ -58,7 +58,8 @@ namespace pcl_ros
 {
 public:
   /** \brief Empty constructor. */
-      EuclideanClusterExtraction () : publish_indices_ (false), max_clusters_ (std::numeric_limits<int>::max ()) {};
+  EuclideanClusterExtraction()
+  : publish_indices_(false), max_clusters_(std::numeric_limits<int>::max()) {}
 
 protected:
   // ROS nodelet attributes
@@ -88,7 +89,9 @@ namespace pcl_ros
     * \param cloud the pointer to the input point cloud
     * \param indices the pointer to the input point cloud indices
     */
-      void input_indices_callback (const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices);
+  void input_indices_callback(
+    const PointCloudConstPtr & cloud,
+    const PointIndicesConstPtr & indices);
 
 private:
   /** \brief The PCL implementation used. */
@@ -98,8 +101,10 @@ namespace pcl_ros
   ros::Subscriber sub_input_;
 
   /** \brief Synchronized input, and indices.*/
-      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > >       sync_input_indices_e_;
-      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > > sync_input_indices_a_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
+    PointIndices>>> sync_input_indices_e_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
+    PointIndices>>> sync_input_indices_a_;
 
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW

pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.hpp

@@ -75,8 +75,10 @@ namespace pcl_ros
   message_filters::Subscriber<PointCloud> sub_hull_filter_;
 
   /** \brief Synchronized input, planar hull, and indices.*/
-      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloud, PointIndices> > > sync_input_hull_indices_e_;
-      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloud, PointIndices> > > sync_input_hull_indices_a_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloud,
+    PointIndices>>> sync_input_hull_indices_e_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
+    PointCloud, PointIndices>>> sync_input_hull_indices_a_;
 
   /** \brief Pointer to a dynamic reconfigure service. */
   boost::shared_ptr<dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig>> srv_;
@@ -115,13 +117,15 @@ namespace pcl_ros
     * \param hull the pointer to the planar hull point cloud
     * \param indices the pointer to the input point cloud indices
     */
-      void input_hull_indices_callback (const PointCloudConstPtr &cloud, 
+  void input_hull_indices_callback(
+    const PointCloudConstPtr & cloud,
     const PointCloudConstPtr & hull,
     const PointIndicesConstPtr & indices);
 
 private:
   /** \brief The PCL implementation used. */
   pcl::ExtractPolygonalPrismData<pcl::PointXYZ> impl_;
+
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };

pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp

@@ -66,7 +66,8 @@ namespace pcl_ros
 
 public:
   /** \brief Constructor. */
-      SACSegmentation () : min_inliers_(0) {}
+  SACSegmentation()
+  : min_inliers_(0) {}
 
   /** \brief Set the input TF frame the data should be transformed into before processing, if input.header.frame_id is different.
     * \param tf_frame the TF frame the input PointCloud should be transformed into before processing
@@ -74,7 +75,7 @@ namespace pcl_ros
   inline void setInputTFframe(std::string tf_frame) {tf_input_frame_ = tf_frame;}
 
   /** \brief Get the TF frame the input PointCloud should be transformed into before processing. */
-      inline std::string getInputTFframe () { return (tf_input_frame_); }
+  inline std::string getInputTFframe() {return tf_input_frame_;}
 
   /** \brief Set the output TF frame the data should be transformed into after processing.
     * \param tf_frame the TF frame the PointCloud should be transformed into after processing
@@ -82,7 +83,7 @@ namespace pcl_ros
   inline void setOutputTFframe(std::string tf_frame) {tf_output_frame_ = tf_frame;}
 
   /** \brief Get the TF frame the PointCloud should be transformed into after processing. */
-      inline std::string getOutputTFframe () { return (tf_output_frame_); }
+  inline std::string getOutputTFframe() {return tf_output_frame_;}
 
 protected:
   // The minimum number of inliers a model must have in order to be considered valid.
@@ -131,7 +132,9 @@ namespace pcl_ros
     * \param cloud the pointer to the input point cloud
     * \param indices the pointer to the input point cloud indices
     */
-      void input_indices_callback (const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices);
+  void input_indices_callback(
+    const PointCloudConstPtr & cloud,
+    const PointIndicesConstPtr & indices);
 
   /** \brief Pointer to a set of indices stored internally.
    * (used when \a latched_indices_ is set).
@@ -167,8 +170,10 @@ namespace pcl_ros
   pcl::SACSegmentation<pcl::PointXYZ> impl_;
 
   /** \brief Synchronized input, and indices.*/
-      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > >       sync_input_indices_e_;
-      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > > sync_input_indices_a_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
+    PointIndices>>> sync_input_indices_e_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
+    PointIndices>>> sync_input_indices_a_;
 
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@@ -195,7 +200,7 @@ namespace pcl_ros
   inline void setInputTFframe(std::string tf_frame) {tf_input_frame_ = tf_frame;}
 
   /** \brief Get the TF frame the input PointCloud should be transformed into before processing. */
-      inline std::string getInputTFframe () { return (tf_input_frame_); }
+  inline std::string getInputTFframe() {return tf_input_frame_;}
 
   /** \brief Set the output TF frame the data should be transformed into after processing.
     * \param tf_frame the TF frame the PointCloud should be transformed into after processing
@@ -203,7 +208,7 @@ namespace pcl_ros
   inline void setOutputTFframe(std::string tf_frame) {tf_output_frame_ = tf_frame;}
 
   /** \brief Get the TF frame the PointCloud should be transformed into after processing. */
-      inline std::string getOutputTFframe () { return (tf_output_frame_); }
+  inline std::string getOutputTFframe() {return tf_output_frame_;}
 
 protected:
   // ROS nodelet attributes
@@ -262,7 +267,8 @@ namespace pcl_ros
     * \param cloud_normals the pointer to the input point cloud normals
     * \param indices the pointer to the input point cloud indices
     */
-      void input_normals_indices_callback (const PointCloudConstPtr &cloud, 
+  void input_normals_indices_callback(
+    const PointCloudConstPtr & cloud,
     const PointCloudNConstPtr & cloud_normals,
     const PointIndicesConstPtr & indices);
 
@@ -274,8 +280,10 @@ namespace pcl_ros
   pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> impl_;
 
   /** \brief Synchronized input, normals, and indices.*/
-      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloudN, PointIndices> > > sync_input_normals_indices_a_;
-      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloudN, PointIndices> > > sync_input_normals_indices_e_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
+    PointCloudN, PointIndices>>> sync_input_normals_indices_a_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloudN,
+    PointIndices>>> sync_input_normals_indices_e_;
 
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW

pcl_ros/include/pcl_ros/segmentation/segment_differences.hpp

@@ -65,15 +65,17 @@ namespace pcl_ros
 
 public:
   /** \brief Empty constructor. */
-      SegmentDifferences () {};
+  SegmentDifferences() {}
 
 protected:
   /** \brief The message filter subscriber for PointCloud2. */
   message_filters::Subscriber<PointCloud> sub_target_filter_;
 
   /** \brief Synchronized input, and planar hull.*/
-      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloud> > > sync_input_target_e_;
-      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloud> > > sync_input_target_a_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
+    PointCloud>>> sync_input_target_e_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
+    PointCloud>>> sync_input_target_a_;
 
   /** \brief Pointer to a dynamic reconfigure service. */
   boost::shared_ptr<dynamic_reconfigure::Server<SegmentDifferencesConfig>> srv_;
@@ -95,7 +97,8 @@ namespace pcl_ros
     * \param cloud the pointer to the input point cloud
     * \param cloud_target the pointcloud that we want to segment \a cloud from
     */
-      void input_target_callback (const PointCloudConstPtr &cloud, 
+  void input_target_callback(
+    const PointCloudConstPtr & cloud,
     const PointCloudConstPtr & cloud_target);
 
 private:

pcl_ros/include/pcl_ros/surface/convex_hull.hpp

@@ -71,7 +71,8 @@ namespace pcl_ros
     * \param cloud the pointer to the input point cloud
     * \param indices the pointer to the input point cloud indices
     */
-      void input_indices_callback (const PointCloudConstPtr &cloud, 
+  void input_indices_callback(
+    const PointCloudConstPtr & cloud,
     const PointIndicesConstPtr & indices);
 
 private:
@@ -85,8 +86,10 @@ namespace pcl_ros
   ros::Publisher pub_plane_;
 
   /** \brief Synchronized input, and indices.*/
-      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > >       sync_input_indices_e_;
-      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > > sync_input_indices_a_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
+    PointIndices>>> sync_input_indices_e_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
+    PointIndices>>> sync_input_indices_a_;
 
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW

pcl_ros/include/pcl_ros/surface/moving_least_squares.hpp

@@ -124,10 +124,10 @@ namespace pcl_ros
     * \param cloud the pointer to the input point cloud
     * \param indices the pointer to the input point cloud indices
     */
-      void input_indices_callback (const PointCloudInConstPtr &cloud, 
+  void input_indices_callback(
+    const PointCloudInConstPtr & cloud,
     const PointIndicesConstPtr & indices);
 
-
 private:
   /** \brief The PCL implementation used. */
   pcl::MovingLeastSquares<PointIn, NormalOut> impl_;
@@ -139,8 +139,10 @@ namespace pcl_ros
   ros::Publisher pub_normals_;
 
   /** \brief Synchronized input, and indices.*/
-      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointIndices> > >       sync_input_indices_e_;
-      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointIndices> > > sync_input_indices_a_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn,
+    PointIndices>>> sync_input_indices_e_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn,
+    PointIndices>>> sync_input_indices_a_;
 
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW

pcl_ros/include/pcl_ros/transforms.hpp

@@ -50,8 +50,10 @@ namespace pcl_ros
   * \param transform a rigid transformation from tf
   * \note calls the Eigen version
   */
-  template <typename PointT> void 
-  transformPointCloudWithNormals (const pcl::PointCloud <PointT> &cloud_in, 
+template<typename PointT>
+void
+transformPointCloudWithNormals(
+  const pcl::PointCloud<PointT> & cloud_in,
   pcl::PointCloud<PointT> & cloud_out,
   const tf::Transform & transform);
 
@@ -61,8 +63,10 @@ namespace pcl_ros
   * \param cloud_out the input point cloud
   * \param tf_listener a TF listener object
   */
-  template <typename PointT> bool 
-  transformPointCloudWithNormals (const std::string &target_frame, 
+template<typename PointT>
+bool
+transformPointCloudWithNormals(
+  const std::string & target_frame,
   const pcl::PointCloud<PointT> & cloud_in,
   pcl::PointCloud<PointT> & cloud_out,
   const tf::TransformListener & tf_listener);
@@ -75,8 +79,10 @@ namespace pcl_ros
   * \param cloud_out the input point cloud
   * \param tf_listener a TF listener object
   */
-   template <typename PointT> bool 
-   transformPointCloudWithNormals (const std::string &target_frame, 
+template<typename PointT>
+bool
+transformPointCloudWithNormals(
+  const std::string & target_frame,
   const ros::Time & target_time,
   const pcl::PointCloud<PointT> & cloud_in,
   const std::string & fixed_frame,
@@ -89,8 +95,10 @@ namespace pcl_ros
   * \param transform a rigid transformation from tf
   * \note calls the Eigen version
   */
-  template <typename PointT> void 
-  transformPointCloud (const pcl::PointCloud <PointT> &cloud_in, 
+template<typename PointT>
+void
+transformPointCloud(
+  const pcl::PointCloud<PointT> & cloud_in,
   pcl::PointCloud<PointT> & cloud_out,
   const tf::Transform & transform);
 
@@ -100,8 +108,10 @@ namespace pcl_ros
   * \param cloud_out the input point cloud
   * \param tf_listener a TF listener object
   */
-  template <typename PointT> bool 
-  transformPointCloud (const std::string &target_frame, 
+template<typename PointT>
+bool
+transformPointCloud(
+  const std::string & target_frame,
   const pcl::PointCloud<PointT> & cloud_in,
   pcl::PointCloud<PointT> & cloud_out,
   const tf::TransformListener & tf_listener);
@@ -114,8 +124,10 @@ namespace pcl_ros
   * \param cloud_out the input point cloud
   * \param tf_listener a TF listener object
   */
-  template <typename PointT> bool 
-  transformPointCloud (const std::string &target_frame, const ros::Time & target_time, 
+template<typename PointT>
+bool
+transformPointCloud(
+  const std::string & target_frame, const ros::Time & target_time,
   const pcl::PointCloud<PointT> & cloud_in,
   const std::string & fixed_frame,
   pcl::PointCloud<PointT> & cloud_out,
@@ -128,7 +140,8 @@ namespace pcl_ros
   * \param tf_listener a TF listener object
   */
 bool
-  transformPointCloud (const std::string &target_frame, 
+transformPointCloud(
+  const std::string & target_frame,
   const sensor_msgs::PointCloud2 & in,
   sensor_msgs::PointCloud2 & out,
   const tf::TransformListener & tf_listener);
@@ -140,7 +153,8 @@ namespace pcl_ros
   * \param out the resultant transformed PointCloud2 dataset
   */
 void
-  transformPointCloud (const std::string &target_frame, 
+transformPointCloud(
+  const std::string & target_frame,
   const tf::Transform & net_transform,
   const sensor_msgs::PointCloud2 & in,
   sensor_msgs::PointCloud2 & out);
@@ -151,7 +165,8 @@ namespace pcl_ros
   * \param out the resultant transformed PointCloud2 dataset
   */
 void
-  transformPointCloud (const Eigen::Matrix4f &transform, 
+transformPointCloud(
+  const Eigen::Matrix4f & transform,
   const sensor_msgs::PointCloud2 & in,
   sensor_msgs::PointCloud2 & out);
 
@@ -164,4 +179,3 @@ namespace pcl_ros
 }
 
 #endif // PCL_ROS_TRANSFORMS_H_
-

pcl_ros/src/pcl_ros/features/boundary.cpp

@@ -47,7 +47,8 @@ pcl_ros::BoundaryEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
 }
 
 void
-pcl_ros::BoundaryEstimation::computePublish (const PointCloudInConstPtr &cloud, 
+pcl_ros::BoundaryEstimation::computePublish(
+  const PointCloudInConstPtr & cloud,
   const PointCloudNConstPtr & normals,
   const PointCloudInConstPtr & surface,
   const IndicesPtr & indices)

pcl_ros/src/pcl_ros/features/feature.cpp

@@ -66,14 +66,16 @@ pcl_ros::Feature::onInit ()
   //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.", getName ().c_str ());
+  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.",
+      getName().c_str());
     return;
   }
-  if (!pnh_->getParam ("spatial_locator", spatial_locator_type_))
-  {
-    NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
+  if (!pnh_->getParam("spatial_locator", spatial_locator_type_)) {
+    NODELET_ERROR(
+      "[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!",
+      getName().c_str());
     return;
   }
 
@@ -82,10 +84,12 @@ pcl_ros::Feature::onInit ()
 
   // Enable the dynamic reconfigure service
   srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig>>(*pnh_);
-  dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind (&Feature::config_callback, this, _1, _2);
+  dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind(
+    &Feature::config_callback, this, _1, _2);
   srv_->setCallback(f);
 
-  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
+  NODELET_DEBUG(
+    "[%s::onInit] Nodelet successfully created with the following parameters:\n"
     " - use_surface    : %s\n"
     " - k_search       : %d\n"
     " - radius_search  : %f\n"
@@ -101,169 +105,209 @@ void
 pcl_ros::Feature::subscribe()
 {
   // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages
-  if (use_indices_ || use_surface_)
-  {
+  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_);
-    else
-      sync_input_surface_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudIn, PointIndices> > >(max_queue_size_);
+    if (approximate_sync_) {
+      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_);
+    }
 
     // Subscribe to the input using a filter
     sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_);
-    if (use_indices_)
-    {
+    if (use_indices_) {
       // 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 (use_surface_) {   // Use both indices and surface
         // 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 (sub_input_filter_, sub_surface_filter_, sub_indices_filter_);
-        else
-          sync_input_surface_indices_e_->connectInput (sub_input_filter_, sub_surface_filter_, sub_indices_filter_);
+        if (approximate_sync_) {
+          sync_input_surface_indices_a_->connectInput(
+            sub_input_filter_, sub_surface_filter_,
+            sub_indices_filter_);
+        } else {
+          sync_input_surface_indices_e_->connectInput(
+            sub_input_filter_, sub_surface_filter_,
+            sub_indices_filter_);
         }
-      else                  // Use only indices
-      {
+      } else {              // Use only indices
         sub_input_filter_.registerCallback(bind(&Feature::input_callback, this, _1));
         // surface not enabled, connect the input-indices duo and register
-        if (approximate_sync_)
-          sync_input_surface_indices_a_->connectInput (sub_input_filter_, nf_pc_, sub_indices_filter_);
-        else
-          sync_input_surface_indices_e_->connectInput (sub_input_filter_, nf_pc_, sub_indices_filter_);
+        if (approximate_sync_) {
+          sync_input_surface_indices_a_->connectInput(
+            sub_input_filter_, nf_pc_,
+            sub_indices_filter_);
+        } else {
+          sync_input_surface_indices_e_->connectInput(
+            sub_input_filter_, nf_pc_,
+            sub_indices_filter_);
         }
       }
-    else                    // Use only surface
-    {
+    } else {                // Use only surface
       sub_input_filter_.registerCallback(bind(&Feature::input_callback, this, _1));
       // indices not enabled, connect the input-surface duo and register
       sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
-      if (approximate_sync_)
+      if (approximate_sync_) {
         sync_input_surface_indices_a_->connectInput(sub_input_filter_, sub_surface_filter_, nf_pi_);
-      else
+      } else {
         sync_input_surface_indices_e_->connectInput(sub_input_filter_, sub_surface_filter_, nf_pi_);
       }
-    // Register callbacks
-    if (approximate_sync_)
-      sync_input_surface_indices_a_->registerCallback (bind (&Feature::input_surface_indices_callback, this, _1, _2, _3));
-    else
-      sync_input_surface_indices_e_->registerCallback (bind (&Feature::input_surface_indices_callback, this, _1, _2, _3));
     }
-  else
+    // Register callbacks
+    if (approximate_sync_) {
+      sync_input_surface_indices_a_->registerCallback(
+        bind(
+          &Feature::input_surface_indices_callback,
+          this, _1, _2, _3));
+    } else {
+      sync_input_surface_indices_e_->registerCallback(
+        bind(
+          &Feature::input_surface_indices_callback,
+          this, _1, _2, _3));
+    }
+  } else {
     // Subscribe in an old fashion to input only (no filters)
-    sub_input_ = pnh_->subscribe<PointCloudIn> ("input", max_queue_size_,  bind (&Feature::input_surface_indices_callback, this, _1, PointCloudInConstPtr (), PointIndicesConstPtr ()));
+    sub_input_ =
+      pnh_->subscribe<PointCloudIn>(
+      "input", max_queue_size_,
+      bind(
+        &Feature::input_surface_indices_callback, this, _1, PointCloudInConstPtr(),
+        PointIndicesConstPtr()));
+  }
 }
 
 ////////////////////////////////////////////////////////////////////////////////////////////
 void
 pcl_ros::Feature::unsubscribe()
 {
-  if (use_indices_ || use_surface_)
-  {
+  if (use_indices_ || use_surface_) {
     sub_input_filter_.unsubscribe();
-    if (use_indices_)
-    {
+    if (use_indices_) {
       sub_indices_filter_.unsubscribe();
-      if (use_surface_)
+      if (use_surface_) {
         sub_surface_filter_.unsubscribe();
       }
-    else
+    } else {
       sub_surface_filter_.unsubscribe();
     }
-  else
+  } else {
     sub_input_.shutdown();
   }
+}
 
 ////////////////////////////////////////////////////////////////////////////////////////////
 void
 pcl_ros::Feature::config_callback(FeatureConfig & config, uint32_t level)
 {
-  if (k_ != config.k_search)
-  {
+  if (k_ != config.k_search) {
     k_ = config.k_search;
-    NODELET_DEBUG ("[config_callback] Setting the number of K nearest neighbors to use for each point: %d.", k_);
+    NODELET_DEBUG(
+      "[config_callback] Setting the number of K nearest neighbors to use for each point: %d.", k_);
   }
-  if (search_radius_ != config.radius_search)
-  {
+  if (search_radius_ != config.radius_search) {
     search_radius_ = config.radius_search;
-    NODELET_DEBUG ("[config_callback] Setting the nearest neighbors search radius for each point: %f.", search_radius_);
+    NODELET_DEBUG(
+      "[config_callback] Setting the nearest neighbors search radius for each point: %f.",
+      search_radius_);
   }
 }
 
 ////////////////////////////////////////////////////////////////////////////////////////////
 void
-pcl_ros::Feature::input_surface_indices_callback (const PointCloudInConstPtr &cloud, 
+pcl_ros::Feature::input_surface_indices_callback(
+  const PointCloudInConstPtr & cloud,
   const PointCloudInConstPtr & cloud_surface, const PointIndicesConstPtr & indices)
 {
   // No subscribers, no work
-  if (pub_output_.getNumSubscribers () <= 0)
+  if (pub_output_.getNumSubscribers() <= 0) {
     return;
+  }
 
   // If cloud is given, check if it's valid
-  if (!isValid (cloud))
-  {
+  if (!isValid(cloud)) {
     NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input!", getName().c_str());
     emptyPublish(cloud);
     return;
   }
 
   // If surface is given, check if it's valid
-  if (cloud_surface && !isValid (cloud_surface, "surface"))
-  {
+  if (cloud_surface && !isValid(cloud_surface, "surface")) {
     NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input surface!", getName().c_str());
     emptyPublish(cloud);
     return;
   }
 
   // If indices are given, check if they are valid
-  if (indices && !isValid (indices))
-  {
+  if (indices && !isValid(indices)) {
     NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input indices!", getName().c_str());
     emptyPublish(cloud);
     return;
   }
 
   /// DEBUG
-  if (cloud_surface)
-    if (indices)
-      NODELET_DEBUG ("[input_surface_indices_callback]\n"
+  if (cloud_surface) {
+    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.",
-                 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 (),
-                 cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), fromPCL(cloud_surface->header).stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (),
-                 indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
-    else
-      NODELET_DEBUG ("[input_surface_indices_callback]\n"
+        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(),
+        cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
+          *cloud_surface).c_str(), fromPCL(
+          cloud_surface->header).stamp.toSec(),
+        cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str(),
+        indices->indices.size(), indices->header.stamp.toSec(),
+        indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
+    } 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.",
-                     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 (),
-                     cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), fromPCL(cloud_surface->header).stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str ());
-
-  else
-    if (indices)
-      NODELET_DEBUG ("[input_surface_indices_callback]\n"
+        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(),
+        cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
+          *cloud_surface).c_str(), fromPCL(
+          cloud_surface->header).stamp.toSec(),
+        cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str());
+    }
+  } 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.",
-                     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 (),
-                     indices->indices.size (), indices->header.stamp.toSec (), 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(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ());
+      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(),
+      indices->indices.size(), indices->header.stamp.toSec(),
+      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(
+        cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
+        "input").c_str());
+  }
   ///
 
 
-  if ((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_, (int)(cloud->width * cloud->height));
+  if ((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_,
+      (int)(cloud->width * cloud->height));
     emptyPublish(cloud);
     return;
   }
 
   // If indices given...
   IndicesPtr vindices;
-  if (indices && !indices->header.frame_id.empty ())
+  if (indices && !indices->header.frame_id.empty()) {
     vindices.reset(new std::vector<int>(indices->indices));
+  }
 
   computePublish(cloud, cloud_surface, vindices);
 }
@@ -287,14 +331,16 @@ pcl_ros::FeatureFromNormals::onInit ()
   //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.", getName ().c_str ());
+  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.",
+      getName().c_str());
     return;
   }
-  if (!pnh_->getParam ("spatial_locator", spatial_locator_type_))
-  {
-    NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
+  if (!pnh_->getParam("spatial_locator", spatial_locator_type_)) {
+    NODELET_ERROR(
+      "[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!",
+      getName().c_str());
     return;
   }
   // ---[ Optional parameters
@@ -302,10 +348,12 @@ pcl_ros::FeatureFromNormals::onInit ()
 
   // Enable the dynamic reconfigure service
   srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig>>(*pnh_);
-  dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind (&FeatureFromNormals::config_callback, this, _1, _2);
+  dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind(
+    &FeatureFromNormals::config_callback, this, _1, _2);
   srv_->setCallback(f);
 
-  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
+  NODELET_DEBUG(
+    "[%s::onInit] Nodelet successfully created with the following parameters:\n"
     " - use_surface    : %s\n"
     " - k_search       : %d\n"
     " - radius_search  : %f\n"
@@ -324,65 +372,92 @@ pcl_ros::FeatureFromNormals::subscribe ()
   sub_normals_filter_.subscribe(*pnh_, "normals", max_queue_size_);
 
   // 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_);
-  else
-    sync_input_normals_surface_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > (max_queue_size_);
+  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_);
+  } else {
+    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
-  if (use_indices_ || use_surface_)
-  {
-    if (use_indices_)
-    {
+  if (use_indices_ || use_surface_) {
+    if (use_indices_) {
       // 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 (use_surface_) {   // Use both indices and surface
         // 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 (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, sub_indices_filter_);
-        else
-          sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, sub_indices_filter_);
+        if (approximate_sync_) {
+          sync_input_normals_surface_indices_a_->connectInput(
+            sub_input_filter_,
+            sub_normals_filter_,
+            sub_surface_filter_,
+            sub_indices_filter_);
+        } else {
+          sync_input_normals_surface_indices_e_->connectInput(
+            sub_input_filter_,
+            sub_normals_filter_,
+            sub_surface_filter_,
+            sub_indices_filter_);
         }
-      else                  // Use only indices
-      {
+      } else {              // Use only indices
         sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1));
-        if (approximate_sync_)
+        if (approximate_sync_) {
           // surface not enabled, connect the input-indices duo and register
-          sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, sub_indices_filter_);
-        else
+          sync_input_normals_surface_indices_a_->connectInput(
+            sub_input_filter_,
+            sub_normals_filter_, nf_pc_,
+            sub_indices_filter_);
+        } else {
           // surface not enabled, connect the input-indices duo and register
-          sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, sub_indices_filter_);
+          sync_input_normals_surface_indices_e_->connectInput(
+            sub_input_filter_,
+            sub_normals_filter_, nf_pc_,
+            sub_indices_filter_);
         }
       }
-    else                    // Use only surface
-    {
+    } else {                // Use only surface
       // indices not enabled, connect the input-surface duo and register
       sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
 
       sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1));
-      if (approximate_sync_)
-        sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, nf_pi_);
-      else
-        sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, nf_pi_);
+      if (approximate_sync_) {
+        sync_input_normals_surface_indices_a_->connectInput(
+          sub_input_filter_, sub_normals_filter_,
+          sub_surface_filter_, nf_pi_);
+      } else {
+        sync_input_normals_surface_indices_e_->connectInput(
+          sub_input_filter_, sub_normals_filter_,
+          sub_surface_filter_, nf_pi_);
       }
     }
-  else
-  {
+  } else {
     sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1));
 
-    if (approximate_sync_)
-      sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, nf_pi_);
-    else
-      sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, nf_pi_);
+    if (approximate_sync_) {
+      sync_input_normals_surface_indices_a_->connectInput(
+        sub_input_filter_, sub_normals_filter_,
+        nf_pc_, nf_pi_);
+    } else {
+      sync_input_normals_surface_indices_e_->connectInput(
+        sub_input_filter_, sub_normals_filter_,
+        nf_pc_, nf_pi_);
+    }
   }
 
   // Register callbacks
-  if (approximate_sync_)
-    sync_input_normals_surface_indices_a_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4));
-  else
-    sync_input_normals_surface_indices_e_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4));
+  if (approximate_sync_) {
+    sync_input_normals_surface_indices_a_->registerCallback(
+      bind(
+        &FeatureFromNormals::
+        input_normals_surface_indices_callback, this, _1, _2, _3, _4));
+  } else {
+    sync_input_normals_surface_indices_e_->registerCallback(
+      bind(
+        &FeatureFromNormals::
+        input_normals_surface_indices_callback, this, _1, _2, _3, _4));
+  }
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
@@ -391,18 +466,17 @@ pcl_ros::FeatureFromNormals::unsubscribe ()
 {
   sub_input_filter_.unsubscribe();
   sub_normals_filter_.unsubscribe();
-  if (use_indices_ || use_surface_)
-  {
-    if (use_indices_)
-    {
+  if (use_indices_ || use_surface_) {
+    if (use_indices_) {
       sub_indices_filter_.unsubscribe();
-      if (use_surface_)
+      if (use_surface_) {
         sub_surface_filter_.unsubscribe();
       }
-    else
+    } else {
       sub_surface_filter_.unsubscribe();
     }
   }
+}
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
@@ -411,86 +485,122 @@ pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback (
   const PointCloudInConstPtr & cloud_surface, const PointIndicesConstPtr & indices)
 {
   // No subscribers, no work
-  if (pub_output_.getNumSubscribers () <= 0)
+  if (pub_output_.getNumSubscribers() <= 0) {
     return;
+  }
 
   // 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;
   }
 
   // If surface is given, check if it's valid
-  if (cloud_surface && !isValid (cloud_surface, "surface"))
-  {
-    NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input surface!", getName ().c_str ());
+  if (cloud_surface && !isValid(cloud_surface, "surface")) {
+    NODELET_ERROR(
+      "[%s::input_normals_surface_indices_callback] Invalid input surface!",
+      getName().c_str());
     emptyPublish(cloud);
     return;
   }
 
   // If indices are given, check if they are valid
-  if (indices && !isValid (indices))
-  {
-    NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input indices!", getName ().c_str ());
+  if (indices && !isValid(indices)) {
+    NODELET_ERROR(
+      "[%s::input_normals_surface_indices_callback] Invalid input indices!",
+      getName().c_str());
     emptyPublish(cloud);
     return;
   }
 
   /// DEBUG
-  if (cloud_surface)
-    if (indices)
-      NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
+  if (cloud_surface) {
+    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.",
         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 ("input").c_str (),
-                     cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), fromPCL(cloud_surface->header).stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (),
-                     cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(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"
+        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(),
+        cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
+          *cloud_surface).c_str(), fromPCL(
+          cloud_surface->header).stamp.toSec(),
+        cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str(),
+        cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
+          *cloud_normals).c_str(), fromPCL(
+          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.\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 ("input").c_str (),
-                     cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), fromPCL(cloud_surface->header).stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (),
-                     cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ());
-  else
-    if (indices)
-      NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
+        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(),
+        cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
+          *cloud_surface).c_str(), fromPCL(
+          cloud_surface->header).stamp.toSec(),
+        cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str(),
+        cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
+          *cloud_normals).c_str(), fromPCL(
+          cloud_normals->header).stamp.toSec(),
+        cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str());
+    }
+  } 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.",
       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 ("input").c_str (),
-                     cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(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"
+      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(),
+      cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
+        *cloud_normals).c_str(), fromPCL(
+        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.",
       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 ("input").c_str (),
-                     cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ());
+      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(),
+      cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
+        *cloud_normals).c_str(), fromPCL(
+        cloud_normals->header).stamp.toSec(),
+      cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str());
+  }
   ///
 
-  if ((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)!", getName ().c_str (), k_, (int)(cloud->width * cloud->height));
+  if ((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)!",
+      getName().c_str(), k_, (int)(cloud->width * cloud->height));
     emptyPublish(cloud);
     return;
   }
 
   // If indices given...
   IndicesPtr vindices;
-  if (indices && !indices->header.frame_id.empty ())
+  if (indices && !indices->header.frame_id.empty()) {
     vindices.reset(new std::vector<int>(indices->indices));
+  }
 
   computePublish(cloud, cloud_normals, cloud_surface, vindices);
 }
-

pcl_ros/src/pcl_ros/features/fpfh.cpp

@@ -47,7 +47,8 @@ pcl_ros::FPFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
 }
 
 void
-pcl_ros::FPFHEstimation::computePublish (const PointCloudInConstPtr &cloud,
+pcl_ros::FPFHEstimation::computePublish(
+  const PointCloudInConstPtr & cloud,
   const PointCloudNConstPtr & normals,
   const PointCloudInConstPtr & surface,
   const IndicesPtr & indices)
@@ -73,4 +74,3 @@ pcl_ros::FPFHEstimation::computePublish (const PointCloudInConstPtr &cloud,
 
 typedef pcl_ros::FPFHEstimation FPFHEstimation;
 PLUGINLIB_EXPORT_CLASS(FPFHEstimation, nodelet::Nodelet)
-

pcl_ros/src/pcl_ros/features/fpfh_omp.cpp

@@ -47,7 +47,8 @@ pcl_ros::FPFHEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud)
 }
 
 void
-pcl_ros::FPFHEstimationOMP::computePublish (const PointCloudInConstPtr &cloud,
+pcl_ros::FPFHEstimationOMP::computePublish(
+  const PointCloudInConstPtr & cloud,
   const PointCloudNConstPtr & normals,
   const PointCloudInConstPtr & surface,
   const IndicesPtr & indices)
@@ -73,4 +74,3 @@ pcl_ros::FPFHEstimationOMP::computePublish (const PointCloudInConstPtr &cloud,
 
 typedef pcl_ros::FPFHEstimationOMP FPFHEstimationOMP;
 PLUGINLIB_EXPORT_CLASS(FPFHEstimationOMP, nodelet::Nodelet)
-

pcl_ros/src/pcl_ros/features/moment_invariants.cpp

@@ -47,7 +47,8 @@ pcl_ros::MomentInvariantsEstimation::emptyPublish (const PointCloudInConstPtr &c
 }
 
 void
-pcl_ros::MomentInvariantsEstimation::computePublish (const PointCloudInConstPtr &cloud,
+pcl_ros::MomentInvariantsEstimation::computePublish(
+  const PointCloudInConstPtr & cloud,
   const PointCloudInConstPtr & surface,
   const IndicesPtr & indices)
 {
@@ -71,4 +72,3 @@ pcl_ros::MomentInvariantsEstimation::computePublish (const PointCloudInConstPtr
 
 typedef pcl_ros::MomentInvariantsEstimation MomentInvariantsEstimation;
 PLUGINLIB_EXPORT_CLASS(MomentInvariantsEstimation, nodelet::Nodelet)
-

pcl_ros/src/pcl_ros/features/normal_3d.cpp

@@ -47,7 +47,8 @@ pcl_ros::NormalEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
 }
 
 void
-pcl_ros::NormalEstimation::computePublish (const PointCloudInConstPtr &cloud,
+pcl_ros::NormalEstimation::computePublish(
+  const PointCloudInConstPtr & cloud,
   const PointCloudInConstPtr & surface,
   const IndicesPtr & indices)
 {
@@ -71,4 +72,3 @@ pcl_ros::NormalEstimation::computePublish (const PointCloudInConstPtr &cloud,
 
 typedef pcl_ros::NormalEstimation NormalEstimation;
 PLUGINLIB_EXPORT_CLASS(NormalEstimation, nodelet::Nodelet)
-

pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp

@@ -47,7 +47,8 @@ pcl_ros::NormalEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud)
 }
 
 void
-pcl_ros::NormalEstimationOMP::computePublish (const PointCloudInConstPtr &cloud,
+pcl_ros::NormalEstimationOMP::computePublish(
+  const PointCloudInConstPtr & cloud,
   const PointCloudInConstPtr & surface,
   const IndicesPtr & indices)
 {
@@ -71,4 +72,3 @@ pcl_ros::NormalEstimationOMP::computePublish (const PointCloudInConstPtr &cloud,
 
 typedef pcl_ros::NormalEstimationOMP NormalEstimationOMP;
 PLUGINLIB_EXPORT_CLASS(NormalEstimationOMP, nodelet::Nodelet)
-

pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp

@@ -49,7 +49,8 @@ pcl_ros::NormalEstimationTBB::emptyPublish (const PointCloudInConstPtr &cloud)
 }
 
 void
-pcl_ros::NormalEstimationTBB::computePublish (const PointCloudInConstPtr &cloud,
+pcl_ros::NormalEstimationTBB::computePublish(
+  const PointCloudInConstPtr & cloud,
   const PointCloudInConstPtr & surface,
   const IndicesPtr & indices)
 {
@@ -78,4 +79,3 @@ typedef pcl_ros::NormalEstimationTBB NormalEstimationTBB;
 PLUGINLIB_EXPORT_CLASS(NormalEstimationTBB, nodelet::Nodelet)
 
 #endif // HAVE_TBB
-

pcl_ros/src/pcl_ros/features/pfh.cpp

@@ -47,7 +47,8 @@ pcl_ros::PFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
 }
 
 void
-pcl_ros::PFHEstimation::computePublish (const PointCloudInConstPtr &cloud,
+pcl_ros::PFHEstimation::computePublish(
+  const PointCloudInConstPtr & cloud,
   const PointCloudNConstPtr & normals,
   const PointCloudInConstPtr & surface,
   const IndicesPtr & indices)
@@ -73,4 +74,3 @@ pcl_ros::PFHEstimation::computePublish (const PointCloudInConstPtr &cloud,
 
 typedef pcl_ros::PFHEstimation PFHEstimation;
 PLUGINLIB_EXPORT_CLASS(PFHEstimation, nodelet::Nodelet)
-

pcl_ros/src/pcl_ros/features/principal_curvatures.cpp

@@ -47,7 +47,8 @@ pcl_ros::PrincipalCurvaturesEstimation::emptyPublish (const PointCloudInConstPtr
 }
 
 void
-pcl_ros::PrincipalCurvaturesEstimation::computePublish (const PointCloudInConstPtr &cloud,
+pcl_ros::PrincipalCurvaturesEstimation::computePublish(
+  const PointCloudInConstPtr & cloud,
   const PointCloudNConstPtr & normals,
   const PointCloudInConstPtr & surface,
   const IndicesPtr & indices)
@@ -73,4 +74,3 @@ pcl_ros::PrincipalCurvaturesEstimation::computePublish (const PointCloudInConstP
 
 typedef pcl_ros::PrincipalCurvaturesEstimation PrincipalCurvaturesEstimation;
 PLUGINLIB_EXPORT_CLASS(PrincipalCurvaturesEstimation, nodelet::Nodelet)
-

pcl_ros/src/pcl_ros/features/shot.cpp

@@ -46,7 +46,8 @@ pcl_ros::SHOTEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
 }
 
 void
-pcl_ros::SHOTEstimation::computePublish (const PointCloudInConstPtr &cloud,
+pcl_ros::SHOTEstimation::computePublish(
+  const PointCloudInConstPtr & cloud,
   const PointCloudNConstPtr & normals,
   const PointCloudInConstPtr & surface,
   const IndicesPtr & indices)
@@ -72,4 +73,3 @@ pcl_ros::SHOTEstimation::computePublish (const PointCloudInConstPtr &cloud,
 
 typedef pcl_ros::SHOTEstimation SHOTEstimation;
 PLUGINLIB_EXPORT_CLASS(SHOTEstimation, nodelet::Nodelet)
-

pcl_ros/src/pcl_ros/features/shot_omp.cpp

@@ -46,7 +46,8 @@ pcl_ros::SHOTEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud)
 }
 
 void
-pcl_ros::SHOTEstimationOMP::computePublish (const PointCloudInConstPtr &cloud,
+pcl_ros::SHOTEstimationOMP::computePublish(
+  const PointCloudInConstPtr & cloud,
   const PointCloudNConstPtr & normals,
   const PointCloudInConstPtr & surface,
   const IndicesPtr & indices)
@@ -72,4 +73,3 @@ pcl_ros::SHOTEstimationOMP::computePublish (const PointCloudInConstPtr &cloud,
 
 typedef pcl_ros::SHOTEstimationOMP SHOTEstimationOMP;
 PLUGINLIB_EXPORT_CLASS(SHOTEstimationOMP, nodelet::Nodelet)
-

pcl_ros/src/pcl_ros/features/vfh.cpp

@@ -47,7 +47,8 @@ pcl_ros::VFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
 }
 
 void
-pcl_ros::VFHEstimation::computePublish (const PointCloudInConstPtr &cloud,
+pcl_ros::VFHEstimation::computePublish(
+  const PointCloudInConstPtr & cloud,
   const PointCloudNConstPtr & normals,
   const PointCloudInConstPtr & surface,
   const IndicesPtr & indices)
@@ -73,4 +74,3 @@ pcl_ros::VFHEstimation::computePublish (const PointCloudInConstPtr &cloud,
 
 typedef pcl_ros::VFHEstimation VFHEstimation;
 PLUGINLIB_EXPORT_CLASS(VFHEstimation, nodelet::Nodelet)
-

pcl_ros/src/pcl_ros/filters/crop_box.cpp

@@ -46,10 +46,11 @@ pcl_ros::CropBox::child_init (ros::NodeHandle &nh, bool &has_service)
   // Enable the dynamic reconfigure service
   has_service = true;
   srv_ = boost::make_shared<dynamic_reconfigure::Server<pcl_ros::CropBoxConfig>>(nh);
-  dynamic_reconfigure::Server<pcl_ros::CropBoxConfig>::CallbackType f = boost::bind (&CropBox::config_callback, this, _1, _2);
+  dynamic_reconfigure::Server<pcl_ros::CropBoxConfig>::CallbackType f = boost::bind(
+    &CropBox::config_callback, this, _1, _2);
   srv_->setCallback(f);
 
-  return (true);
+  return true;
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
@@ -67,49 +68,54 @@ pcl_ros::CropBox::config_callback (pcl_ros::CropBoxConfig &config, uint32_t leve
   new_max_point << config.max_x, config.max_y, config.max_z, 0.0;
 
   // Check the current values for minimum point
-  if (min_point != new_min_point)
-  {
-    NODELET_DEBUG ("[%s::config_callback] Setting the minimum point to: %f %f %f.", getName ().c_str (), new_min_point(0),new_min_point(1),new_min_point(2));
+  if (min_point != new_min_point) {
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting the minimum point to: %f %f %f.",
+      getName().c_str(), new_min_point(0), new_min_point(1), new_min_point(2));
     // Set the filter min point if different
     impl_.setMin(new_min_point);
   }
   // Check the current values for the maximum point
- if (max_point != new_max_point)
-  {
-    NODELET_DEBUG ("[%s::config_callback] Setting the maximum point to: %f %f %f.", getName ().c_str (), new_max_point(0),new_max_point(1),new_max_point(2));
+  if (max_point != new_max_point) {
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting the maximum point to: %f %f %f.",
+      getName().c_str(), new_max_point(0), new_max_point(1), new_max_point(2));
     // Set the filter max point if different
     impl_.setMax(new_max_point);
   }
 
   // Check the current value for keep_organized
-  if (impl_.getKeepOrganized () != config.keep_organized)
-  {
-    NODELET_DEBUG ("[%s::config_callback] Setting the filter keep_organized value to: %s.", getName ().c_str (), config.keep_organized ? "true" : "false");
+  if (impl_.getKeepOrganized() != config.keep_organized) {
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting the filter keep_organized value to: %s.",
+      getName().c_str(), config.keep_organized ? "true" : "false");
     // Call the virtual method in the child
     impl_.setKeepOrganized(config.keep_organized);
   }
 
   // Check the current value for the negative flag
-  if (impl_.getNegative() != config.negative)
-  {
-    NODELET_DEBUG ("[%s::config_callback] Setting the filter negative flag to: %s.", getName ().c_str (), config.negative ? "true" : "false");
+  if (impl_.getNegative() != config.negative) {
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting the filter negative flag to: %s.",
+      getName().c_str(), config.negative ? "true" : "false");
     // Call the virtual method in the child
     impl_.setNegative(config.negative);
   }
 
   // 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)
-  {
+  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 ());
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting the input TF frame to: %s.",
+      getName().c_str(), tf_input_frame_.c_str());
   }
-  if (tf_output_frame_ != config.output_frame)
-  {
+  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 ());
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting the output TF frame to: %s.",
+      getName().c_str(), tf_output_frame_.c_str());
   }
 }
 
 typedef pcl_ros::CropBox CropBox;
 PLUGINLIB_EXPORT_CLASS(CropBox, nodelet::Nodelet);
-

pcl_ros/src/pcl_ros/filters/extract_indices.cpp

@@ -45,11 +45,12 @@ pcl_ros::ExtractIndices::child_init (ros::NodeHandle &nh, bool &has_service)
   has_service = true;
 
   srv_ = boost::make_shared<dynamic_reconfigure::Server<pcl_ros::ExtractIndicesConfig>>(nh);
-  dynamic_reconfigure::Server<pcl_ros::ExtractIndicesConfig>::CallbackType f =  boost::bind (&ExtractIndices::config_callback, this, _1, _2);
+  dynamic_reconfigure::Server<pcl_ros::ExtractIndicesConfig>::CallbackType f = boost::bind(
+    &ExtractIndices::config_callback, this, _1, _2);
   srv_->setCallback(f);
 
   use_indices_ = true;
-  return (true);
+  return true;
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
@@ -58,13 +59,13 @@ pcl_ros::ExtractIndices::config_callback (pcl_ros::ExtractIndicesConfig &config,
 {
   boost::mutex::scoped_lock lock(mutex_);
 
-  if (impl_.getNegative () != config.negative)
-  {
+  if (impl_.getNegative() != config.negative) {
     impl_.setNegative(config.negative);
-    NODELET_DEBUG ("[%s::config_callback] Setting the extraction to: %s.", getName ().c_str (), (config.negative ? "indices" : "everything but the indices"));
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting the extraction to: %s.", getName().c_str(),
+      (config.negative ? "indices" : "everything but the indices"));
   }
 }
 
 typedef pcl_ros::ExtractIndices ExtractIndices;
 PLUGINLIB_EXPORT_CLASS(ExtractIndices, nodelet::Nodelet);
-

pcl_ros/src/pcl_ros/filters/features/boundary.cpp

@@ -47,7 +47,8 @@ pcl_ros::BoundaryEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
 }
 
 void
-pcl_ros::BoundaryEstimation::computePublish (const PointCloudInConstPtr &cloud, 
+pcl_ros::BoundaryEstimation::computePublish(
+  const PointCloudInConstPtr & cloud,
   const PointCloudNConstPtr & normals,
   const PointCloudInConstPtr & surface,
   const IndicesPtr & indices)

pcl_ros/src/pcl_ros/filters/features/feature.cpp

@@ -66,14 +66,16 @@ pcl_ros::Feature::onInit ()
   //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.", getName ().c_str ());
+  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.",
+      getName().c_str());
     return;
   }
-  if (!pnh_->getParam ("spatial_locator", spatial_locator_type_))
-  {
-    NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
+  if (!pnh_->getParam("spatial_locator", spatial_locator_type_)) {
+    NODELET_ERROR(
+      "[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!",
+      getName().c_str());
     return;
   }
 
@@ -82,64 +84,85 @@ pcl_ros::Feature::onInit ()
 
   // Enable the dynamic reconfigure service
   srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig>>(*pnh_);
-  dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind (&Feature::config_callback, this, _1, _2);
+  dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind(
+    &Feature::config_callback, this, _1, _2);
   srv_->setCallback(f);
 
   // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages
-  if (use_indices_ || use_surface_)
-  {
+  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_);
-    else
-      sync_input_surface_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudIn, PointIndices> > >(max_queue_size_);
+    if (approximate_sync_) {
+      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_);
+    }
 
     // Subscribe to the input using a filter
     sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_);
-    if (use_indices_)
-    {
+    if (use_indices_) {
       // 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 (use_surface_) {   // Use both indices and surface
         // 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 (sub_input_filter_, sub_surface_filter_, sub_indices_filter_);
-        else
-          sync_input_surface_indices_e_->connectInput (sub_input_filter_, sub_surface_filter_, sub_indices_filter_);
+        if (approximate_sync_) {
+          sync_input_surface_indices_a_->connectInput(
+            sub_input_filter_, sub_surface_filter_,
+            sub_indices_filter_);
+        } else {
+          sync_input_surface_indices_e_->connectInput(
+            sub_input_filter_, sub_surface_filter_,
+            sub_indices_filter_);
         }
-      else                  // Use only indices
-      {
+      } else {              // Use only indices
         sub_input_filter_.registerCallback(bind(&Feature::input_callback, this, _1));
         // surface not enabled, connect the input-indices duo and register
-        if (approximate_sync_)
-          sync_input_surface_indices_a_->connectInput (sub_input_filter_, nf_pc_, sub_indices_filter_);
-        else
-          sync_input_surface_indices_e_->connectInput (sub_input_filter_, nf_pc_, sub_indices_filter_);
+        if (approximate_sync_) {
+          sync_input_surface_indices_a_->connectInput(
+            sub_input_filter_, nf_pc_,
+            sub_indices_filter_);
+        } else {
+          sync_input_surface_indices_e_->connectInput(
+            sub_input_filter_, nf_pc_,
+            sub_indices_filter_);
         }
       }
-    else                    // Use only surface
-    {
+    } else {                // Use only surface
       sub_input_filter_.registerCallback(bind(&Feature::input_callback, this, _1));
       // indices not enabled, connect the input-surface duo and register
       sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
-      if (approximate_sync_)
+      if (approximate_sync_) {
         sync_input_surface_indices_a_->connectInput(sub_input_filter_, sub_surface_filter_, nf_pi_);
-      else
+      } else {
         sync_input_surface_indices_e_->connectInput(sub_input_filter_, sub_surface_filter_, nf_pi_);
       }
-    // Register callbacks
-    if (approximate_sync_)
-      sync_input_surface_indices_a_->registerCallback (bind (&Feature::input_surface_indices_callback, this, _1, _2, _3));
-    else
-      sync_input_surface_indices_e_->registerCallback (bind (&Feature::input_surface_indices_callback, this, _1, _2, _3));
     }
-  else
+    // Register callbacks
+    if (approximate_sync_) {
+      sync_input_surface_indices_a_->registerCallback(
+        bind(
+          &Feature::input_surface_indices_callback,
+          this, _1, _2, _3));
+    } else {
+      sync_input_surface_indices_e_->registerCallback(
+        bind(
+          &Feature::input_surface_indices_callback,
+          this, _1, _2, _3));
+    }
+  } else {
     // Subscribe in an old fashion to input only (no filters)
-    sub_input_ = pnh_->subscribe<PointCloudIn> ("input", max_queue_size_,  bind (&Feature::input_surface_indices_callback, this, _1, PointCloudInConstPtr (), PointIndicesConstPtr ()));
+    sub_input_ =
+      pnh_->subscribe<PointCloudIn>(
+      "input", max_queue_size_,
+      bind(
+        &Feature::input_surface_indices_callback, this, _1, PointCloudInConstPtr(),
+        PointIndicesConstPtr()));
+  }
 
-  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
+  NODELET_DEBUG(
+    "[%s::onInit] Nodelet successfully created with the following parameters:\n"
     " - use_surface    : %s\n"
     " - k_search       : %d\n"
     " - radius_search  : %f\n"
@@ -152,91 +175,113 @@ pcl_ros::Feature::onInit ()
 void
 pcl_ros::Feature::config_callback(FeatureConfig & config, uint32_t level)
 {
-  if (k_ != config.k_search)
-  {
+  if (k_ != config.k_search) {
     k_ = config.k_search;
-    NODELET_DEBUG ("[config_callback] Setting the number of K nearest neighbors to use for each point: %d.", k_);
+    NODELET_DEBUG(
+      "[config_callback] Setting the number of K nearest neighbors to use for each point: %d.", k_);
   }
-  if (search_radius_ != config.radius_search)
-  {
+  if (search_radius_ != config.radius_search) {
     search_radius_ = config.radius_search;
-    NODELET_DEBUG ("[config_callback] Setting the nearest neighbors search radius for each point: %f.", search_radius_);
+    NODELET_DEBUG(
+      "[config_callback] Setting the nearest neighbors search radius for each point: %f.",
+      search_radius_);
   }
 }
 
 ////////////////////////////////////////////////////////////////////////////////////////////
 void
-pcl_ros::Feature::input_surface_indices_callback (const PointCloudInConstPtr &cloud, 
+pcl_ros::Feature::input_surface_indices_callback(
+  const PointCloudInConstPtr & cloud,
   const PointCloudInConstPtr & cloud_surface, const PointIndicesConstPtr & indices)
 {
   // No subscribers, no work
-  if (pub_output_.getNumSubscribers () <= 0)
+  if (pub_output_.getNumSubscribers() <= 0) {
     return;
+  }
 
   // If cloud is given, check if it's valid
-  if (!isValid (cloud))
-  {
+  if (!isValid(cloud)) {
     NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input!", getName().c_str());
     emptyPublish(cloud);
     return;
   }
 
   // If surface is given, check if it's valid
-  if (cloud_surface && !isValid (cloud_surface, "surface"))
-  {
+  if (cloud_surface && !isValid(cloud_surface, "surface")) {
     NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input surface!", getName().c_str());
     emptyPublish(cloud);
     return;
   }
 
   // If indices are given, check if they are valid
-  if (indices && !isValid (indices))
-  {
+  if (indices && !isValid(indices)) {
     NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input indices!", getName().c_str());
     emptyPublish(cloud);
     return;
   }
 
   /// DEBUG
-  if (cloud_surface)
-    if (indices)
-      NODELET_DEBUG ("[input_surface_indices_callback]\n"
+  if (cloud_surface) {
+    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.",
-                 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_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), cloud_surface->header.stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (),
-                 indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
-    else
-      NODELET_DEBUG ("[input_surface_indices_callback]\n"
+        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_surface->width * cloud_surface->height, pcl::getFieldsList(
+          *cloud_surface).c_str(),
+        cloud_surface->header.stamp.toSec(),
+        cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str(),
+        indices->indices.size(), indices->header.stamp.toSec(),
+        indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
+    } 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.",
-                     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_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), cloud_surface->header.stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str ());
-
-  else
-    if (indices)
-      NODELET_DEBUG ("[input_surface_indices_callback]\n"
+        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_surface->width * cloud_surface->height, pcl::getFieldsList(
+          *cloud_surface).c_str(),
+        cloud_surface->header.stamp.toSec(),
+        cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str());
+    }
+  } 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.",
-                     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 ());
-    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, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").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(),
+      indices->indices.size(), indices->header.stamp.toSec(),
+      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,
+      cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
+        "input").c_str());
+  }
   ///
 
 
-  if ((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_, (int)(cloud->width * cloud->height));
+  if ((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_,
+      (int)(cloud->width * cloud->height));
     emptyPublish(cloud);
     return;
   }
 
   // If indices given...
   IndicesPtr vindices;
-  if (indices && !indices->header.frame_id.empty ())
+  if (indices && !indices->header.frame_id.empty()) {
     vindices.reset(new std::vector<int>(indices->indices));
+  }
 
   computePublish(cloud, cloud_surface, vindices);
 }
@@ -260,14 +305,16 @@ pcl_ros::FeatureFromNormals::onInit ()
   //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.", getName ().c_str ());
+  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.",
+      getName().c_str());
     return;
   }
-  if (!pnh_->getParam ("spatial_locator", spatial_locator_type_))
-  {
-    NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
+  if (!pnh_->getParam("spatial_locator", spatial_locator_type_)) {
+    NODELET_ERROR(
+      "[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!",
+      getName().c_str());
     return;
   }
   // ---[ Optional parameters
@@ -278,71 +325,100 @@ pcl_ros::FeatureFromNormals::onInit ()
 
   // Enable the dynamic reconfigure service
   srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig>>(*pnh_);
-  dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind (&FeatureFromNormals::config_callback, this, _1, _2);
+  dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind(
+    &FeatureFromNormals::config_callback, this, _1, _2);
   srv_->setCallback(f);
 
   // 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_);
-  else
-    sync_input_normals_surface_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > (max_queue_size_);
+  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_);
+  } else {
+    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
-  if (use_indices_ || use_surface_)
-  {
-    if (use_indices_)
-    {
+  if (use_indices_ || use_surface_) {
+    if (use_indices_) {
       // 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 (use_surface_) {   // Use both indices and surface
         // 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 (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, sub_indices_filter_);
-        else
-          sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, sub_indices_filter_);
+        if (approximate_sync_) {
+          sync_input_normals_surface_indices_a_->connectInput(
+            sub_input_filter_,
+            sub_normals_filter_,
+            sub_surface_filter_,
+            sub_indices_filter_);
+        } else {
+          sync_input_normals_surface_indices_e_->connectInput(
+            sub_input_filter_,
+            sub_normals_filter_,
+            sub_surface_filter_,
+            sub_indices_filter_);
         }
-      else                  // Use only indices
-      {
+      } else {              // Use only indices
         sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1));
-        if (approximate_sync_)
+        if (approximate_sync_) {
           // surface not enabled, connect the input-indices duo and register
-          sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, sub_indices_filter_);
-        else
+          sync_input_normals_surface_indices_a_->connectInput(
+            sub_input_filter_,
+            sub_normals_filter_, nf_pc_,
+            sub_indices_filter_);
+        } else {
           // surface not enabled, connect the input-indices duo and register
-          sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, sub_indices_filter_);
+          sync_input_normals_surface_indices_e_->connectInput(
+            sub_input_filter_,
+            sub_normals_filter_, nf_pc_,
+            sub_indices_filter_);
         }
       }
-    else                    // Use only surface
-    {
+    } else {                // Use only surface
       // indices not enabled, connect the input-surface duo and register
       sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
 
       sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1));
-      if (approximate_sync_)
-        sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, nf_pi_);
-      else
-        sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, nf_pi_);
+      if (approximate_sync_) {
+        sync_input_normals_surface_indices_a_->connectInput(
+          sub_input_filter_, sub_normals_filter_,
+          sub_surface_filter_, nf_pi_);
+      } else {
+        sync_input_normals_surface_indices_e_->connectInput(
+          sub_input_filter_, sub_normals_filter_,
+          sub_surface_filter_, nf_pi_);
       }
     }
-  else
-  {
+  } else {
     sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1));
 
-    if (approximate_sync_)
-      sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, nf_pi_);
-    else
-      sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, nf_pi_);
+    if (approximate_sync_) {
+      sync_input_normals_surface_indices_a_->connectInput(
+        sub_input_filter_, sub_normals_filter_,
+        nf_pc_, nf_pi_);
+    } else {
+      sync_input_normals_surface_indices_e_->connectInput(
+        sub_input_filter_, sub_normals_filter_,
+        nf_pc_, nf_pi_);
+    }
   }
 
   // Register callbacks
-  if (approximate_sync_)
-    sync_input_normals_surface_indices_a_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4));
-  else
-    sync_input_normals_surface_indices_e_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4));
+  if (approximate_sync_) {
+    sync_input_normals_surface_indices_a_->registerCallback(
+      bind(
+        &FeatureFromNormals::
+        input_normals_surface_indices_callback, this, _1, _2, _3, _4));
+  } else {
+    sync_input_normals_surface_indices_e_->registerCallback(
+      bind(
+        &FeatureFromNormals::
+        input_normals_surface_indices_callback, this, _1, _2, _3, _4));
+  }
 
-  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
+  NODELET_DEBUG(
+    "[%s::onInit] Nodelet successfully created with the following parameters:\n"
     " - use_surface    : %s\n"
     " - k_search       : %d\n"
     " - radius_search  : %f\n"
@@ -358,86 +434,122 @@ pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback (
   const PointCloudInConstPtr & cloud_surface, const PointIndicesConstPtr & indices)
 {
   // No subscribers, no work
-  if (pub_output_.getNumSubscribers () <= 0)
+  if (pub_output_.getNumSubscribers() <= 0) {
     return;
+  }
 
   // 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;
   }
 
   // If surface is given, check if it's valid
-  if (cloud_surface && !isValid (cloud_surface, "surface"))
-  {
-    NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input surface!", getName ().c_str ());
+  if (cloud_surface && !isValid(cloud_surface, "surface")) {
+    NODELET_ERROR(
+      "[%s::input_normals_surface_indices_callback] Invalid input surface!",
+      getName().c_str());
     emptyPublish(cloud);
     return;
   }
 
   // If indices are given, check if they are valid
-  if (indices && !isValid (indices))
-  {
-    NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input indices!", getName ().c_str ());
+  if (indices && !isValid(indices)) {
+    NODELET_ERROR(
+      "[%s::input_normals_surface_indices_callback] Invalid input indices!",
+      getName().c_str());
     emptyPublish(cloud);
     return;
   }
 
   /// DEBUG
-  if (cloud_surface)
-    if (indices)
-      NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
+  if (cloud_surface) {
+    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.",
         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_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), cloud_surface->header.stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").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 (),
-                     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"
+        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_surface->width * cloud_surface->height, pcl::getFieldsList(
+          *cloud_surface).c_str(),
+        cloud_surface->header.stamp.toSec(),
+        cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").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(),
+        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.\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_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), cloud_surface->header.stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").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 ());
-  else
-    if (indices)
-      NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
+        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_surface->width * cloud_surface->height, pcl::getFieldsList(
+          *cloud_surface).c_str(),
+        cloud_surface->header.stamp.toSec(),
+        cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").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());
+    }
+  } 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.",
       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 (),
-                     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"
+      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(),
+      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.",
       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->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());
+  }
   ///
 
-  if ((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)!", getName ().c_str (), k_, (int)(cloud->width * cloud->height));
+  if ((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)!",
+      getName().c_str(), k_, (int)(cloud->width * cloud->height));
     emptyPublish(cloud);
     return;
   }
 
   // If indices given...
   IndicesPtr vindices;
-  if (indices && !indices->header.frame_id.empty ())
+  if (indices && !indices->header.frame_id.empty()) {
     vindices.reset(new std::vector<int>(indices->indices));
+  }
 
   computePublish(cloud, cloud_normals, cloud_surface, vindices);
 }
-

pcl_ros/src/pcl_ros/filters/features/fpfh.cpp

@@ -47,7 +47,8 @@ pcl_ros::FPFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
 }
 
 void
-pcl_ros::FPFHEstimation::computePublish (const PointCloudInConstPtr &cloud,
+pcl_ros::FPFHEstimation::computePublish(
+  const PointCloudInConstPtr & cloud,
   const PointCloudNConstPtr & normals,
   const PointCloudInConstPtr & surface,
   const IndicesPtr & indices)
@@ -76,4 +77,3 @@ pcl_ros::FPFHEstimation::computePublish (const PointCloudInConstPtr &cloud,
 
 typedef pcl_ros::FPFHEstimation FPFHEstimation;
 PLUGINLIB_EXPORT_CLASS(FPFHEstimation, nodelet::Nodelet);
-

pcl_ros/src/pcl_ros/filters/features/fpfh_omp.cpp

@@ -47,7 +47,8 @@ pcl_ros::FPFHEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud)
 }
 
 void
-pcl_ros::FPFHEstimationOMP::computePublish (const PointCloudInConstPtr &cloud,
+pcl_ros::FPFHEstimationOMP::computePublish(
+  const PointCloudInConstPtr & cloud,
   const PointCloudNConstPtr & normals,
   const PointCloudInConstPtr & surface,
   const IndicesPtr & indices)
@@ -76,4 +77,3 @@ pcl_ros::FPFHEstimationOMP::computePublish (const PointCloudInConstPtr &cloud,
 
 typedef pcl_ros::FPFHEstimationOMP FPFHEstimationOMP;
 PLUGINLIB_EXPORT_CLASS(FPFHEstimationOMP, nodelet::Nodelet);
-

pcl_ros/src/pcl_ros/filters/features/moment_invariants.cpp

@@ -47,7 +47,8 @@ pcl_ros::MomentInvariantsEstimation::emptyPublish (const PointCloudInConstPtr &c
 }
 
 void
-pcl_ros::MomentInvariantsEstimation::computePublish (const PointCloudInConstPtr &cloud,
+pcl_ros::MomentInvariantsEstimation::computePublish(
+  const PointCloudInConstPtr & cloud,
   const PointCloudInConstPtr & surface,
   const IndicesPtr & indices)
 {
@@ -74,4 +75,3 @@ pcl_ros::MomentInvariantsEstimation::computePublish (const PointCloudInConstPtr
 
 typedef pcl_ros::MomentInvariantsEstimation MomentInvariantsEstimation;
 PLUGINLIB_EXPORT_CLASS(MomentInvariantsEstimation, nodelet::Nodelet);
-

pcl_ros/src/pcl_ros/filters/features/normal_3d.cpp

@@ -47,7 +47,8 @@ pcl_ros::NormalEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
 }
 
 void
-pcl_ros::NormalEstimation::computePublish (const PointCloudInConstPtr &cloud,
+pcl_ros::NormalEstimation::computePublish(
+  const PointCloudInConstPtr & cloud,
   const PointCloudInConstPtr & surface,
   const IndicesPtr & indices)
 {
@@ -74,4 +75,3 @@ pcl_ros::NormalEstimation::computePublish (const PointCloudInConstPtr &cloud,
 
 typedef pcl_ros::NormalEstimation NormalEstimation;
 PLUGINLIB_EXPORT_CLASS(NormalEstimation, nodelet::Nodelet);
-

pcl_ros/src/pcl_ros/filters/features/normal_3d_omp.cpp

@@ -47,7 +47,8 @@ pcl_ros::NormalEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud)
 }
 
 void
-pcl_ros::NormalEstimationOMP::computePublish (const PointCloudInConstPtr &cloud,
+pcl_ros::NormalEstimationOMP::computePublish(
+  const PointCloudInConstPtr & cloud,
   const PointCloudInConstPtr & surface,
   const IndicesPtr & indices)
 {
@@ -74,4 +75,3 @@ pcl_ros::NormalEstimationOMP::computePublish (const PointCloudInConstPtr &cloud,
 
 typedef pcl_ros::NormalEstimationOMP NormalEstimationOMP;
 PLUGINLIB_EXPORT_CLASS(NormalEstimationOMP, nodelet::Nodelet);
-

pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp

@@ -49,7 +49,8 @@ pcl_ros::NormalEstimationTBB::emptyPublish (const PointCloudInConstPtr &cloud)
 }
 
 void
-pcl_ros::NormalEstimationTBB::computePublish (const PointCloudInConstPtr &cloud,
+pcl_ros::NormalEstimationTBB::computePublish(
+  const PointCloudInConstPtr & cloud,
   const PointCloudInConstPtr & surface,
   const IndicesPtr & indices)
 {
@@ -78,4 +79,3 @@ typedef pcl_ros::NormalEstimationTBB NormalEstimationTBB;
 PLUGINLIB_EXPORT_CLASS(NormalEstimationTBB, nodelet::Nodelet);
 
 #endif // HAVE_TBB
-

pcl_ros/src/pcl_ros/filters/features/pfh.cpp

@@ -47,7 +47,8 @@ pcl_ros::PFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
 }
 
 void
-pcl_ros::PFHEstimation::computePublish (const PointCloudInConstPtr &cloud,
+pcl_ros::PFHEstimation::computePublish(
+  const PointCloudInConstPtr & cloud,
   const PointCloudNConstPtr & normals,
   const PointCloudInConstPtr & surface,
   const IndicesPtr & indices)
@@ -76,4 +77,3 @@ pcl_ros::PFHEstimation::computePublish (const PointCloudInConstPtr &cloud,
 
 typedef pcl_ros::PFHEstimation PFHEstimation;
 PLUGINLIB_EXPORT_CLASS(PFHEstimation, nodelet::Nodelet);
-

pcl_ros/src/pcl_ros/filters/features/principal_curvatures.cpp

@@ -47,7 +47,8 @@ pcl_ros::PrincipalCurvaturesEstimation::emptyPublish (const PointCloudInConstPtr
 }
 
 void
-pcl_ros::PrincipalCurvaturesEstimation::computePublish (const PointCloudInConstPtr &cloud,
+pcl_ros::PrincipalCurvaturesEstimation::computePublish(
+  const PointCloudInConstPtr & cloud,
   const PointCloudNConstPtr & normals,
   const PointCloudInConstPtr & surface,
   const IndicesPtr & indices)
@@ -76,4 +77,3 @@ pcl_ros::PrincipalCurvaturesEstimation::computePublish (const PointCloudInConstP
 
 typedef pcl_ros::PrincipalCurvaturesEstimation PrincipalCurvaturesEstimation;
 PLUGINLIB_EXPORT_CLASS(PrincipalCurvaturesEstimation, nodelet::Nodelet);
-

pcl_ros/src/pcl_ros/filters/features/vfh.cpp

@@ -47,7 +47,8 @@ pcl_ros::VFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
 }
 
 void
-pcl_ros::VFHEstimation::computePublish (const PointCloudInConstPtr &cloud,
+pcl_ros::VFHEstimation::computePublish(
+  const PointCloudInConstPtr & cloud,
   const PointCloudNConstPtr & normals,
   const PointCloudInConstPtr & surface,
   const IndicesPtr & indices)
@@ -76,4 +77,3 @@ pcl_ros::VFHEstimation::computePublish (const PointCloudInConstPtr &cloud,
 
 typedef pcl_ros::VFHEstimation VFHEstimation;
 PLUGINLIB_EXPORT_CLASS(VFHEstimation, nodelet::Nodelet);
-

pcl_ros/src/pcl_ros/filters/filter.cpp

@@ -69,27 +69,34 @@ pcl_ros::Filter::computePublish (const PointCloud2::ConstPtr &input, const Indic
 
   PointCloud2::Ptr 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 ());
+  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());
     // 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_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());
       return;
     }
     cloud_tf.reset(new PointCloud2(cloud_transformed));
   }
-  if (tf_output_frame_.empty () && output.header.frame_id != tf_input_orig_frame_)
+  if (tf_output_frame_.empty() && output.header.frame_id != tf_input_orig_frame_) {
     // no tf_output_frame given, transform the dataset to its original frame
-  {
-    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 ());
+    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());
     // Convert the cloud into the different frame
     PointCloud2 cloud_transformed;
-    if (!pcl_ros::transformPointCloud (tf_input_orig_frame_, output, cloud_transformed, tf_listener_))
+    if (!pcl_ros::transformPointCloud(
+        tf_input_orig_frame_, output, cloud_transformed,
+        tf_listener_))
     {
-      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 ());
+      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());
       return;
     }
     cloud_tf.reset(new PointCloud2(cloud_transformed));
@@ -107,42 +114,42 @@ void
 pcl_ros::Filter::subscribe()
 {
   // If we're supposed to look for PointIndices (indices)
-  if (use_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_);
 
-    if (approximate_sync_)
-    {
-      sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, pcl_msgs::PointIndices> > >(max_queue_size_);
+    if (approximate_sync_) {
+      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, pcl_msgs::PointIndices> > >(max_queue_size_);
+    } else {
+      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));
     }
-  }
-  else
+  } else {
     // Subscribe in an old fashion to input only (no filters)
-    sub_input_ = pnh_->subscribe<sensor_msgs::PointCloud2> ("input", max_queue_size_,  bind (&Filter::input_indices_callback, this, _1, pcl_msgs::PointIndicesConstPtr ()));
+    sub_input_ =
+      pnh_->subscribe<sensor_msgs::PointCloud2>(
+      "input", max_queue_size_,
+      bind(&Filter::input_indices_callback, this, _1, pcl_msgs::PointIndicesConstPtr()));
+  }
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
 pcl_ros::Filter::unsubscribe()
 {
-  if (use_indices_)
-  {
+  if (use_indices_) {
     sub_input_filter_.unsubscribe();
     sub_indices_filter_.unsubscribe();
-  }
-  else
+  } else {
     sub_input_.shutdown();
   }
+}
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
@@ -153,8 +160,7 @@ pcl_ros::Filter::onInit ()
 
   // Call the child's local init
   bool has_service = false;
-  if (!child_init (*pnh_, has_service))
-  {
+  if (!child_init(*pnh_, has_service)) {
     NODELET_ERROR("[%s::onInit] Initialization failed.", getName().c_str());
     return;
   }
@@ -162,10 +168,10 @@ pcl_ros::Filter::onInit ()
   pub_output_ = advertise<PointCloud2>(*pnh_, "output", max_queue_size_);
 
   // Enable the dynamic reconfigure service
-  if (!has_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);
+    dynamic_reconfigure::Server<pcl_ros::FilterConfig>::CallbackType f = boost::bind(
+      &Filter::config_callback, this, _1, _2);
     srv_->setCallback(f);
   }
 
@@ -177,71 +183,83 @@ 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
-  if (tf_input_frame_ != config.input_frame)
-  {
+  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 ());
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting the input TF frame to: %s.",
+      getName().c_str(), tf_input_frame_.c_str());
   }
-  if (tf_output_frame_ != config.output_frame)
-  {
+  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 ());
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting the output TF frame to: %s.",
+      getName().c_str(), tf_output_frame_.c_str());
   }
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
-pcl_ros::Filter::input_indices_callback (const PointCloud2::ConstPtr &cloud, const PointIndicesConstPtr &indices)
+pcl_ros::Filter::input_indices_callback(
+  const PointCloud2::ConstPtr & cloud,
+  const PointIndicesConstPtr & indices)
 {
   // If cloud is given, check if it's valid
-  if (!isValid (cloud))
-  {
+  if (!isValid(cloud)) {
     NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str());
     return;
   }
   // If indices are given, check if they are valid
-  if (indices && !isValid (indices))
-  {
+  if (indices && !isValid(indices)) {
     NODELET_ERROR("[%s::input_indices_callback] Invalid indices!", getName().c_str());
     return;
   }
 
   /// DEBUG
-  if (indices)
-    NODELET_DEBUG ("[%s::input_indices_callback]\n"
+  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(),
-                   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 ());
-  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 ());
+      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());
+  } 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());
+  }
   ///
 
   // Check whether the user has given a different input TF frame
   tf_input_orig_frame_ = cloud->header.frame_id;
   PointCloud2::ConstPtr 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 ());
+  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());
     // 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_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());
       return;
     }
     cloud_tf = boost::make_shared<PointCloud2>(cloud_transformed);
-  }
-  else
+  } else {
     cloud_tf = cloud;
+  }
 
   // Need setInputCloud () here because we have to extract x/y/z
   IndicesPtr vindices;
-  if (indices)
+  if (indices) {
     vindices.reset(new std::vector<int>(indices->indices));
+  }
 
   computePublish(cloud_tf, vindices);
 }
-

pcl_ros/src/pcl_ros/filters/passthrough.cpp

@@ -45,10 +45,11 @@ pcl_ros::PassThrough::child_init (ros::NodeHandle &nh, bool &has_service)
   // Enable the dynamic reconfigure service
   has_service = true;
   srv_ = boost::make_shared<dynamic_reconfigure::Server<pcl_ros::FilterConfig>>(nh);
-  dynamic_reconfigure::Server<pcl_ros::FilterConfig>::CallbackType f = boost::bind (&PassThrough::config_callback, this, _1, _2);
+  dynamic_reconfigure::Server<pcl_ros::FilterConfig>::CallbackType f = boost::bind(
+    &PassThrough::config_callback, this, _1, _2);
   srv_->setCallback(f);
 
-  return (true);
+  return true;
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
@@ -61,60 +62,66 @@ pcl_ros::PassThrough::config_callback (pcl_ros::FilterConfig &config, uint32_t l
   impl_.getFilterLimits(filter_min, filter_max);
 
   // Check the current values for filter min-max
-  if (filter_min != config.filter_limit_min)
-  {
+  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.", getName ().c_str (), filter_min);
+    NODELET_DEBUG(
+      "[%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);
   }
   // Check the current values for filter min-max
-  if (filter_max != config.filter_limit_max)
-  {
+  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.", getName ().c_str (), filter_max);
+    NODELET_DEBUG(
+      "[%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 ();
-  if (impl_.getFilterFieldName () != config.filter_field_name)
-  {
+  if (impl_.getFilterFieldName() != config.filter_field_name) {
     // Set the filter field if different
     impl_.setFilterFieldName(config.filter_field_name);
-    NODELET_DEBUG ("[%s::config_callback] Setting the filter field name to: %s.", getName ().c_str (), config.filter_field_name.c_str ());
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting the filter field name to: %s.",
+      getName().c_str(), config.filter_field_name.c_str());
   }
 
   // Check the current value for keep_organized
-  if (impl_.getKeepOrganized () != config.keep_organized)
-  {
-    NODELET_DEBUG ("[%s::config_callback] Setting the filter keep_organized value to: %s.", getName ().c_str (), config.keep_organized ? "true" : "false");
+  if (impl_.getKeepOrganized() != config.keep_organized) {
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting the filter keep_organized value to: %s.",
+      getName().c_str(), config.keep_organized ? "true" : "false");
     // Call the virtual method in the child
     impl_.setKeepOrganized(config.keep_organized);
   }
 
   // Check the current value for the negative flag
-  if (impl_.getFilterLimitsNegative () != config.filter_limit_negative)
-  {
-    NODELET_DEBUG ("[%s::config_callback] Setting the filter negative flag to: %s.", getName ().c_str (), config.filter_limit_negative ? "true" : "false");
+  if (impl_.getFilterLimitsNegative() != config.filter_limit_negative) {
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting the filter negative flag to: %s.",
+      getName().c_str(), config.filter_limit_negative ? "true" : "false");
     // Call the virtual method in the child
     impl_.setFilterLimitsNegative(config.filter_limit_negative);
   }
 
   // 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)
-  {
+  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 ());
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting the input TF frame to: %s.",
+      getName().c_str(), tf_input_frame_.c_str());
   }
-  if (tf_output_frame_ != config.output_frame)
-  {
+  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 ());
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting the output TF frame to: %s.",
+      getName().c_str(), tf_output_frame_.c_str());
   }
 }
 
 typedef pcl_ros::PassThrough PassThrough;
 PLUGINLIB_EXPORT_CLASS(PassThrough, nodelet::Nodelet);
-

pcl_ros/src/pcl_ros/filters/project_inliers.cpp

@@ -48,9 +48,10 @@ pcl_ros::ProjectInliers::onInit ()
   // ---[ Mandatory parameters
   // The type of model to use (user given parameter).
   int model_type;
-  if (!pnh_->getParam ("model_type", model_type))
-  {
-    NODELET_ERROR ("[%s::onInit] Need a 'model_type' parameter to be set before continuing!", getName ().c_str ());
+  if (!pnh_->getParam("model_type", model_type)) {
+    NODELET_ERROR(
+      "[%s::onInit] Need a 'model_type' parameter to be set before continuing!",
+      getName().c_str());
     return;
   }
   // ---[ Optional parameters
@@ -68,7 +69,8 @@ pcl_ros::ProjectInliers::onInit ()
   // Subscribe to the input using a filter
   sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_);
 
-  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
+  NODELET_DEBUG(
+    "[%s::onInit] Nodelet successfully created with the following parameters:\n"
     " - model_type      : %d\n"
     " - copy_all_data   : %s\n"
     " - copy_all_fields : %s",
@@ -96,17 +98,22 @@ 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_);
+  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_->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_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_->connectInput(sub_input_filter_, sub_indices_filter_, sub_model_);
-    sync_input_indices_model_e_->registerCallback (bind (&ProjectInliers::input_indices_model_callback, this, _1, _2, _3));
+    sync_input_indices_model_e_->registerCallback(
+      bind(
+        &ProjectInliers::input_indices_model_callback,
+        this, _1, _2, _3));
   }
 }
 
@@ -125,33 +132,39 @@ pcl_ros::ProjectInliers::unsubscribe ()
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
-pcl_ros::ProjectInliers::input_indices_model_callback (const PointCloud2::ConstPtr &cloud, 
+pcl_ros::ProjectInliers::input_indices_model_callback(
+  const PointCloud2::ConstPtr & cloud,
   const PointIndicesConstPtr & indices,
   const ModelCoefficientsConstPtr & model)
 {
-  if (pub_output_.getNumSubscribers () <= 0)
+  if (pub_output_.getNumSubscribers() <= 0) {
     return;
+  }
 
-  if (!isValid (model) || !isValid (indices) || !isValid (cloud))
-  {
+  if (!isValid(model) || !isValid(indices) || !isValid(cloud)) {
     NODELET_ERROR("[%s::input_indices_model_callback] Invalid input!", getName().c_str());
     return;
   }
 
-  NODELET_DEBUG ("[%s::input_indices_model_callback]\n"
+  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.",
     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 (),
-                 indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("inliers").c_str (),
-                 model->values.size (), model->header.stamp.toSec (), model->header.frame_id.c_str (), pnh_->resolveName ("model").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(),
+    indices->indices.size(), indices->header.stamp.toSec(),
+    indices->header.frame_id.c_str(), pnh_->resolveName("inliers").c_str(),
+    model->values.size(), model->header.stamp.toSec(),
+    model->header.frame_id.c_str(), pnh_->resolveName("model").c_str());
 
   tf_input_orig_frame_ = cloud->header.frame_id;
 
   IndicesPtr vindices;
-  if (indices)
+  if (indices) {
     vindices.reset(new std::vector<int>(indices->indices));
+  }
 
   model_ = model;
   computePublish(cloud, vindices);
@@ -159,4 +172,3 @@ pcl_ros::ProjectInliers::input_indices_model_callback (const PointCloud2::ConstP
 
 typedef pcl_ros::ProjectInliers ProjectInliers;
 PLUGINLIB_EXPORT_CLASS(ProjectInliers, nodelet::Nodelet);
-

pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp

@@ -45,28 +45,33 @@ pcl_ros::RadiusOutlierRemoval::child_init (ros::NodeHandle &nh, bool &has_servic
   // Enable the dynamic reconfigure service
   has_service = true;
   srv_ = boost::make_shared<dynamic_reconfigure::Server<pcl_ros::RadiusOutlierRemovalConfig>>(nh);
-  dynamic_reconfigure::Server<pcl_ros::RadiusOutlierRemovalConfig>::CallbackType f = boost::bind (&RadiusOutlierRemoval::config_callback, this, _1, _2);
+  dynamic_reconfigure::Server<pcl_ros::RadiusOutlierRemovalConfig>::CallbackType f = boost::bind(
+    &RadiusOutlierRemoval::config_callback, this, _1, _2);
   srv_->setCallback(f);
 
-  return (true);
+  return true;
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
-pcl_ros::RadiusOutlierRemoval::config_callback (pcl_ros::RadiusOutlierRemovalConfig &config, uint32_t level)
+pcl_ros::RadiusOutlierRemoval::config_callback(
+  pcl_ros::RadiusOutlierRemovalConfig & config,
+  uint32_t level)
 {
   boost::mutex::scoped_lock lock(mutex_);
 
-  if (impl_.getMinNeighborsInRadius () != config.min_neighbors)
-  {
+  if (impl_.getMinNeighborsInRadius() != config.min_neighbors) {
     impl_.setMinNeighborsInRadius(config.min_neighbors);
-    NODELET_DEBUG ("[%s::config_callback] Setting the number of neighbors in radius: %d.", getName ().c_str (), config.min_neighbors);
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting the number of neighbors in radius: %d.",
+      getName().c_str(), config.min_neighbors);
   }
 
-  if (impl_.getRadiusSearch () != config.radius_search)
-  {
+  if (impl_.getRadiusSearch() != config.radius_search) {
     impl_.setRadiusSearch(config.radius_search);
-    NODELET_DEBUG ("[%s::config_callback] Setting the radius to search neighbors: %f.", getName ().c_str (), config.radius_search);
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting the radius to search neighbors: %f.",
+      getName().c_str(), config.radius_search);
   }
 
 }
@@ -74,4 +79,3 @@ pcl_ros::RadiusOutlierRemoval::config_callback (pcl_ros::RadiusOutlierRemovalCon
 
 typedef pcl_ros::RadiusOutlierRemoval RadiusOutlierRemoval;
 PLUGINLIB_EXPORT_CLASS(RadiusOutlierRemoval, nodelet::Nodelet);
-

pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp

@@ -44,38 +44,43 @@ pcl_ros::StatisticalOutlierRemoval::child_init (ros::NodeHandle &nh, bool &has_s
 {
   // Enable the dynamic reconfigure service
   has_service = true;
-  srv_ = boost::make_shared <dynamic_reconfigure::Server<pcl_ros::StatisticalOutlierRemovalConfig> > (nh);
-  dynamic_reconfigure::Server<pcl_ros::StatisticalOutlierRemovalConfig>::CallbackType f = boost::bind (&StatisticalOutlierRemoval::config_callback, this, _1, _2);
+  srv_ = boost::make_shared<dynamic_reconfigure::Server<pcl_ros::StatisticalOutlierRemovalConfig>>(
+    nh);
+  dynamic_reconfigure::Server<pcl_ros::StatisticalOutlierRemovalConfig>::CallbackType f =
+    boost::bind(&StatisticalOutlierRemoval::config_callback, this, _1, _2);
   srv_->setCallback(f);
 
-  return (true);
+  return true;
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
-pcl_ros::StatisticalOutlierRemoval::config_callback (pcl_ros::StatisticalOutlierRemovalConfig &config, uint32_t level)
+pcl_ros::StatisticalOutlierRemoval::config_callback(
+  pcl_ros::StatisticalOutlierRemovalConfig & config, uint32_t level)
 {
   boost::mutex::scoped_lock lock(mutex_);
 
-  if (impl_.getMeanK () != config.mean_k)
-  {
+  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.", getName ().c_str (), config.mean_k);
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting the number of points (k) to use for mean distance estimation to: %d.",
+      getName().c_str(), config.mean_k);
   }
 
-  if (impl_.getStddevMulThresh () != config.stddev)
-  {
+  if (impl_.getStddevMulThresh() != config.stddev) {
     impl_.setStddevMulThresh(config.stddev);
-    NODELET_DEBUG ("[%s::config_callback] Setting the standard deviation multiplier threshold to: %f.", getName ().c_str (), config.stddev);
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting the standard deviation multiplier threshold to: %f.",
+      getName().c_str(), config.stddev);
   }
 
-  if (impl_.getNegative () != config.negative)
-  {
+  if (impl_.getNegative() != config.negative) {
     impl_.setNegative(config.negative);
-    NODELET_DEBUG ("[%s::config_callback] Returning only inliers: %s.", getName ().c_str (), config.negative ? "false" : "true");
+    NODELET_DEBUG(
+      "[%s::config_callback] Returning only inliers: %s.",
+      getName().c_str(), config.negative ? "false" : "true");
   }
 }
 
 typedef pcl_ros::StatisticalOutlierRemoval StatisticalOutlierRemoval;
 PLUGINLIB_EXPORT_CLASS(StatisticalOutlierRemoval, nodelet::Nodelet);
-

pcl_ros/src/pcl_ros/filters/voxel_grid.cpp

@@ -45,15 +45,17 @@ pcl_ros::VoxelGrid::child_init (ros::NodeHandle &nh, bool &has_service)
   // Enable the dynamic reconfigure service
   has_service = true;
   srv_ = boost::make_shared<dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig>>(nh);
-  dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig>::CallbackType f = boost::bind (&VoxelGrid::config_callback, this, _1, _2);
+  dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig>::CallbackType f = boost::bind(
+    &VoxelGrid::config_callback, this, _1, _2);
   srv_->setCallback(f);
 
-  return (true);
+  return true;
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
-pcl_ros::VoxelGrid::filter (const PointCloud2::ConstPtr &input, 
+pcl_ros::VoxelGrid::filter(
+  const PointCloud2::ConstPtr & input,
   const IndicesPtr & indices,
   PointCloud2 & output)
 {
@@ -75,8 +77,7 @@ pcl_ros::VoxelGrid::config_callback (pcl_ros::VoxelGridConfig &config, uint32_t
 
   Eigen::Vector3f leaf_size = impl_.getLeafSize();
 
-  if (leaf_size[0] != config.leaf_size)
-  {
+  if (leaf_size[0] != config.leaf_size) {
     leaf_size.setConstant(config.leaf_size);
     NODELET_DEBUG("[config_callback] Setting the downsampling leaf size to: %f.", leaf_size[0]);
     impl_.setLeafSize(leaf_size[0], leaf_size[1], leaf_size[2]);
@@ -84,44 +85,47 @@ pcl_ros::VoxelGrid::config_callback (pcl_ros::VoxelGridConfig &config, uint32_t
 
   double filter_min, filter_max;
   impl_.getFilterLimits(filter_min, filter_max);
-  if (filter_min != config.filter_limit_min)
-  {
+  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.", filter_min);
+    NODELET_DEBUG(
+      "[config_callback] Setting the minimum filtering value a point will be considered from to: %f.",
+      filter_min);
   }
-  if (filter_max != config.filter_limit_max)
-  {
+  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.", filter_max);
+    NODELET_DEBUG(
+      "[config_callback] Setting the maximum filtering value a point will be considered from to: %f.",
+      filter_max);
   }
   impl_.setFilterLimits(filter_min, filter_max);
 
-  if (impl_.getFilterLimitsNegative () != config.filter_limit_negative)
-  {
+  if (impl_.getFilterLimitsNegative() != config.filter_limit_negative) {
     impl_.setFilterLimitsNegative(config.filter_limit_negative);
-    NODELET_DEBUG ("[%s::config_callback] Setting the filter negative flag to: %s.", getName ().c_str (), config.filter_limit_negative ? "true" : "false");
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting the filter negative flag to: %s.",
+      getName().c_str(), config.filter_limit_negative ? "true" : "false");
   }
 
-  if (impl_.getFilterFieldName () != config.filter_field_name)
-  {
+  if (impl_.getFilterFieldName() != config.filter_field_name) {
     impl_.setFilterFieldName(config.filter_field_name);
-    NODELET_DEBUG ("[config_callback] Setting the filter field name to: %s.", config.filter_field_name.c_str ());
+    NODELET_DEBUG(
+      "[config_callback] Setting the filter field name to: %s.",
+      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
-  if (tf_input_frame_ != config.input_frame)
-  {
+  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());
   }
-  if (tf_output_frame_ != config.output_frame)
-  {
+  if (tf_output_frame_ != config.output_frame) {
     tf_output_frame_ = config.output_frame;
-    NODELET_DEBUG ("[config_callback] Setting the output TF frame to: %s.", tf_output_frame_.c_str ());
+    NODELET_DEBUG(
+      "[config_callback] Setting the output TF frame to: %s.",
+      tf_output_frame_.c_str());
   }
   // ]---
 }
 
 typedef pcl_ros::VoxelGrid VoxelGrid;
 PLUGINLIB_EXPORT_CLASS(VoxelGrid, nodelet::Nodelet);
-

pcl_ros/src/pcl_ros/io/bag_io.cpp

@@ -42,21 +42,19 @@
 bool
 pcl_ros::BAGReader::open(const std::string & file_name, const std::string & topic_name)
 {
-  try
-  {
+  try {
     bag_.open(file_name, rosbag::bagmode::Read);
     view_.addQuery(bag_, rosbag::TopicQuery(topic_name));
 
-    if (view_.size () == 0)
-      return (false);
+    if (view_.size() == 0) {
+      return false;
+    }
 
     it_ = view_.begin();
+  } catch (rosbag::BagException & e) {
+    return false;
   }
-  catch (rosbag::BagException &e)
-  {
-    return (false);
-  }
-  return (true);
+  return true;
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
@@ -66,13 +64,11 @@ pcl_ros::BAGReader::onInit ()
   boost::shared_ptr<ros::NodeHandle> pnh_;
   pnh_.reset(new ros::NodeHandle(getMTPrivateNodeHandle()));
   // ---[ Mandatory parameters
-  if (!pnh_->getParam ("file_name", file_name_))
-  {
+  if (!pnh_->getParam("file_name", file_name_)) {
     NODELET_ERROR("[onInit] Need a 'file_name' parameter to be set before continuing!");
     return;
   }
-   if (!pnh_->getParam ("topic_name", topic_name_))
-  {
+  if (!pnh_->getParam("topic_name", topic_name_)) {
     NODELET_ERROR("[onInit] Need a 'topic_name' parameter to be set before continuing!");
     return;
   }
@@ -83,22 +79,26 @@ pcl_ros::BAGReader::onInit ()
 
   ros::Publisher pub_output = pnh_->advertise<sensor_msgs::PointCloud2>("output", max_queue_size);
 
-  NODELET_DEBUG ("[onInit] Nodelet successfully created with the following parameters:\n"
+  NODELET_DEBUG(
+    "[onInit] Nodelet successfully created with the following parameters:\n"
     " - file_name    : %s\n"
     " - topic_name   : %s",
     file_name_.c_str(), topic_name_.c_str());
 
-  if (!open (file_name_, topic_name_))
+  if (!open(file_name_, topic_name_)) {
     return;
+  }
   PointCloud output;
   output_ = boost::make_shared<PointCloud>(output);
   output_->header.stamp = ros::Time::now();
 
   // Continous publishing enabled?
-  while (pnh_->ok ())
-  {
+  while (pnh_->ok()) {
     PointCloudConstPtr cloud = getNextCloud();
-    NODELET_DEBUG ("Publishing data (%d points) on topic %s in frame %s.", output_->width * output_->height, pnh_->resolveName ("output").c_str (), output_->header.frame_id.c_str ());
+    NODELET_DEBUG(
+      "Publishing data (%d points) on topic %s in frame %s.",
+      output_->width * output_->height, pnh_->resolveName(
+        "output").c_str(), output_->header.frame_id.c_str());
     output_->header.stamp = ros::Time::now();
 
     pub_output.publish(output_);
@@ -110,4 +110,3 @@ pcl_ros::BAGReader::onInit ()
 
 typedef pcl_ros::BAGReader BAGReader;
 PLUGINLIB_EXPORT_CLASS(BAGReader, nodelet::Nodelet);
-

pcl_ros/src/pcl_ros/io/concatenate_data.cpp

@@ -52,29 +52,24 @@ pcl_ros::PointCloudConcatenateDataSynchronizer::onInit ()
   pnh_->getParam("output_frame", output_frame_);
   pnh_->getParam("approximate_sync", approximate_sync_);
 
-  if (output_frame_.empty ())
-  {
+  if (output_frame_.empty()) {
     NODELET_ERROR("[onInit] Need an 'output_frame' parameter to be set before continuing!");
     return;
   }
 
-  if (!pnh_->getParam ("input_topics", input_topics_))
-  {
+  if (!pnh_->getParam("input_topics", input_topics_)) {
     NODELET_ERROR("[onInit] Need a 'input_topics' parameter to be set before continuing!");
     return;
   }
-  if (input_topics_.getType () != XmlRpc::XmlRpcValue::TypeArray)
-  {
+  if (input_topics_.getType() != XmlRpc::XmlRpcValue::TypeArray) {
     NODELET_ERROR("[onInit] Invalid 'input_topics' parameter given!");
     return;
   }
-  if (input_topics_.size () == 1)
-  {
+  if (input_topics_.size() == 1) {
     NODELET_ERROR("[onInit] Only one topic given. Need at least two topics to continue.");
     return;
   }
-  if (input_topics_.size () > 8)
-  {
+  if (input_topics_.size() > 8) {
     NODELET_ERROR("[onInit] More than 8 topics passed!");
     return;
   }
@@ -93,90 +88,124 @@ void
 pcl_ros::PointCloudConcatenateDataSynchronizer::subscribe()
 {
   ROS_INFO_STREAM("Subscribing to " << input_topics_.size() << " user given topics as inputs:");
-  for (int d = 0; d < input_topics_.size (); ++d)
+  for (int d = 0; d < input_topics_.size(); ++d) {
     ROS_INFO_STREAM(" - " << (std::string)(input_topics_[d]));
+  }
 
   // Subscribe to the filters
   filters_.resize(input_topics_.size());
 
   // 8 topics
-  if (approximate_sync_)
-    ts_a_.reset (new message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, PointCloud2, PointCloud2,
+  if (approximate_sync_) {
+    ts_a_.reset(
+      new message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, PointCloud2,
+      PointCloud2,
       PointCloud2, PointCloud2, PointCloud2,
       PointCloud2, PointCloud2>
       >(maximum_queue_size_));
-  else
-    ts_e_.reset (new message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointCloud2, PointCloud2,
+  } else {
+    ts_e_.reset(
+      new message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointCloud2,
+      PointCloud2,
       PointCloud2, PointCloud2, PointCloud2,
       PointCloud2, PointCloud2>
       >(maximum_queue_size_));
+  }
 
   // First input_topics_.size () filters are valid
-  for (int d = 0; d < input_topics_.size (); ++d)
-  {
+  for (int d = 0; d < input_topics_.size(); ++d) {
     filters_[d].reset(new message_filters::Subscriber<PointCloud2>());
     filters_[d]->subscribe(*pnh_, (std::string)(input_topics_[d]), maximum_queue_size_);
   }
 
   // Bogus null filter
-  filters_[0]->registerCallback (bind (&PointCloudConcatenateDataSynchronizer::input_callback, this, _1));
+  filters_[0]->registerCallback(
+    bind(
+      &PointCloudConcatenateDataSynchronizer::input_callback, this,
+      _1));
 
-  switch (input_topics_.size ())
-  {
+  switch (input_topics_.size()) {
     case 2:
       {
-      if (approximate_sync_)
+        if (approximate_sync_) {
           ts_a_->connectInput(*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_);
-      else
+        } else {
           ts_e_->connectInput(*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_);
+        }
         break;
       }
     case 3:
       {
-      if (approximate_sync_)
+        if (approximate_sync_) {
           ts_a_->connectInput(*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_);
-      else
+        } else {
           ts_e_->connectInput(*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_);
+        }
         break;
       }
     case 4:
       {
-      if (approximate_sync_)
-        ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_);
-      else
-        ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_);
+        if (approximate_sync_) {
+          ts_a_->connectInput(
+            *filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_,
+            nf_);
+        } else {
+          ts_e_->connectInput(
+            *filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_,
+            nf_);
+        }
         break;
       }
     case 5:
       {
-      if (approximate_sync_)
-        ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_);
-      else
-        ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_);
+        if (approximate_sync_) {
+          ts_a_->connectInput(
+            *filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
+            nf_, nf_, nf_);
+        } else {
+          ts_e_->connectInput(
+            *filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
+            nf_, nf_, nf_);
+        }
         break;
       }
     case 6:
       {
-      if (approximate_sync_)
-        ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_);
-      else
-        ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_);
+        if (approximate_sync_) {
+          ts_a_->connectInput(
+            *filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
+            *filters_[5], nf_, nf_);
+        } else {
+          ts_e_->connectInput(
+            *filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
+            *filters_[5], nf_, nf_);
+        }
         break;
       }
     case 7:
       {
-      if (approximate_sync_)
-        ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_);
-      else
-        ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_);
+        if (approximate_sync_) {
+          ts_a_->connectInput(
+            *filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
+            *filters_[5], *filters_[6], nf_);
+        } else {
+          ts_e_->connectInput(
+            *filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
+            *filters_[5], *filters_[6], nf_);
+        }
         break;
       }
     case 8:
       {
-      if (approximate_sync_)
-        ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]);
-      else
-        ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]);
+        if (approximate_sync_) {
+          ts_a_->connectInput(
+            *filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
+            *filters_[5], *filters_[6], *filters_[7]);
+        } else {
+          ts_e_->connectInput(
+            *filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
+            *filters_[5], *filters_[6], *filters_[7]);
+        }
         break;
       }
     default:
@@ -186,40 +215,51 @@ pcl_ros::PointCloudConcatenateDataSynchronizer::subscribe ()
       }
   }
 
-  if (approximate_sync_)
-    ts_a_->registerCallback (boost::bind (&PointCloudConcatenateDataSynchronizer::input, this, _1, _2, _3, _4, _5, _6, _7, _8));
-  else
-    ts_e_->registerCallback (boost::bind (&PointCloudConcatenateDataSynchronizer::input, this, _1, _2, _3, _4, _5, _6, _7, _8));
+  if (approximate_sync_) {
+    ts_a_->registerCallback(
+      boost::bind(
+        &PointCloudConcatenateDataSynchronizer::input, this, _1, _2,
+        _3, _4, _5, _6, _7, _8));
+  } else {
+    ts_e_->registerCallback(
+      boost::bind(
+        &PointCloudConcatenateDataSynchronizer::input, this, _1, _2,
+        _3, _4, _5, _6, _7, _8));
+  }
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
 pcl_ros::PointCloudConcatenateDataSynchronizer::unsubscribe()
 {
-  for (int d = 0; d < filters_.size (); ++d)
-  {
+  for (int d = 0; d < filters_.size(); ++d) {
     filters_[d]->unsubscribe();
   }
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
-pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (const PointCloud2 &in1, const PointCloud2 &in2, PointCloud2 &out)
+pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(
+  const PointCloud2 & in1,
+  const PointCloud2 & in2,
+  PointCloud2 & out)
 {
   //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());
 
   // Transform the point clouds into the specified output frame
-  if (output_frame_ != in1.header.frame_id)
+  if (output_frame_ != in1.header.frame_id) {
     pcl_ros::transformPointCloud(output_frame_, in1, *in1_t, tf_);
-  else
+  } else {
     in1_t = boost::make_shared<PointCloud2>(in1);
+  }
 
-  if (output_frame_ != in2.header.frame_id)
+  if (output_frame_ != in2.header.frame_id) {
     pcl_ros::transformPointCloud(output_frame_, in2, *in2_t, tf_);
-  else
+  } else {
     in2_t = boost::make_shared<PointCloud2>(in2);
+  }
 
   // Concatenate the results
   pcl::concatenatePointCloud(*in1_t, *in2_t, out);
@@ -238,22 +278,17 @@ pcl_ros::PointCloudConcatenateDataSynchronizer::input (
   PointCloud2::Ptr out1(new PointCloud2());
   PointCloud2::Ptr out2(new PointCloud2());
   pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(*in1, *in2, *out1);
-  if (in3 && in3->width * in3->height > 0)
-  {
+  if (in3 && in3->width * in3->height > 0) {
     pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(*out1, *in3, *out2);
     out1 = out2;
-    if (in4 && in4->width * in4->height > 0)
-    {
+    if (in4 && in4->width * in4->height > 0) {
       pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(*out2, *in4, *out1);
-      if (in5 && in5->width * in5->height > 0)
-      {
+      if (in5 && in5->width * in5->height > 0) {
         pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(*out1, *in5, *out2);
         out1 = out2;
-        if (in6 && in6->width * in6->height > 0)
-        {
+        if (in6 && in6->width * in6->height > 0) {
           pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(*out2, *in6, *out1);
-          if (in7 && in7->width * in7->height > 0)
-          {
+          if (in7 && in7->width * in7->height > 0) {
             pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(*out1, *in7, *out2);
             out1 = out2;
           }
@@ -266,4 +301,3 @@ pcl_ros::PointCloudConcatenateDataSynchronizer::input (
 
 typedef pcl_ros::PointCloudConcatenateDataSynchronizer PointCloudConcatenateDataSynchronizer;
 PLUGINLIB_EXPORT_CLASS(PointCloudConcatenateDataSynchronizer, nodelet::Nodelet);
-

pcl_ros/src/pcl_ros/io/concatenate_fields.cpp

@@ -50,13 +50,11 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit ()
   nodelet_topic_tools::NodeletLazy::onInit();
 
   // ---[ Mandatory parameters
-  if (!pnh_->getParam ("input_messages", input_messages_))
-  {
+  if (!pnh_->getParam("input_messages", input_messages_)) {
     NODELET_ERROR("[onInit] Need a 'input_messages' parameter to be set before continuing!");
     return;
   }
-  if (input_messages_ < 2)
-  {
+  if (input_messages_ < 2) {
     NODELET_ERROR("[onInit] Invalid 'input_messages' parameter given!");
     return;
   }
@@ -72,7 +70,9 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit ()
 void
 pcl_ros::PointCloudConcatenateFieldsSynchronizer::subscribe()
 {
-  sub_input_ = pnh_->subscribe ("input", maximum_queue_size_,  &PointCloudConcatenateFieldsSynchronizer::input_callback, this);
+  sub_input_ = pnh_->subscribe(
+    "input", maximum_queue_size_,
+    &PointCloudConcatenateFieldsSynchronizer::input_callback, this);
 }
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -86,24 +86,28 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::unsubscribe ()
 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.",
-                 cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ());
+  NODELET_DEBUG(
+    "[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());
 
   // Erase old data in the queue
-  if (maximum_seconds_ > 0 && queue_.size () > 0)
+  if (maximum_seconds_ > 0 && queue_.size() > 0) {
+    while (fabs( ( (*queue_.begin()).first - cloud->header.stamp).toSec() ) > maximum_seconds_ &&
+      queue_.size() > 0)
     {
-    while (fabs ( ( (*queue_.begin ()).first - cloud->header.stamp).toSec () ) > maximum_seconds_ && queue_.size () > 0)
-    {
-      NODELET_WARN ("[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 () ));
+      NODELET_WARN(
+        "[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());
     }
   }
 
   // Push back new data
   queue_[cloud->header.stamp].push_back(cloud);
-  if ((int)queue_[cloud->header.stamp].size () >= input_messages_)
-  {
+  if ((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];
@@ -112,13 +116,13 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback (const PointClo
     int data_size = cloud_out.data.size();
     int nr_fields = cloud_out.fields.size();
     int nr_points = cloud_out.width * cloud_out.height;
-    for (size_t i = 1; i < clouds.size (); ++i)
-    {
-      assert (clouds[i]->data.size () / (clouds[i]->width * clouds[i]->height) == clouds[i]->point_step);
+    for (size_t i = 1; i < clouds.size(); ++i) {
+      assert(
+        clouds[i]->data.size() / (clouds[i]->width * clouds[i]->height) == clouds[i]->point_step);
 
-      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)!", 
+      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)!",
           i, clouds[i]->width, clouds[i]->height, cloud_out.width, cloud_out.height);
         break;
       }
@@ -129,9 +133,9 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback (const PointClo
 
       // Concatenate fields
       cloud_out.fields.resize(nr_fields + clouds[i]->fields.size());
-      int delta_offset = cloud_out.fields[nr_fields - 1].offset + pcl::getFieldSize (cloud_out.fields[nr_fields - 1].datatype);
-      for (size_t d = 0; d < clouds[i]->fields.size (); ++d)
-      {
+      int delta_offset = cloud_out.fields[nr_fields - 1].offset + pcl::getFieldSize(
+        cloud_out.fields[nr_fields - 1].datatype);
+      for (size_t d = 0; d < clouds[i]->fields.size(); ++d) {
         cloud_out.fields[nr_fields + d] = clouds[i]->fields[d];
         cloud_out.fields[nr_fields + d].offset += delta_offset;
       }
@@ -143,12 +147,12 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback (const PointClo
 
     // Iterate over each point and perform the appropriate memcpys
     int point_offset = 0;
-    for (int cp = 0; cp < nr_points; ++cp)
-    {
-      for (size_t i = 0; i < clouds.size (); ++i)
-      {
+    for (int cp = 0; cp < nr_points; ++cp) {
+      for (size_t i = 0; i < clouds.size(); ++i) {
         // Copy each individual point
-        memcpy (&cloud_out.data[point_offset], &clouds[i]->data[cp * clouds[i]->point_step], clouds[i]->point_step);
+        memcpy(
+          &cloud_out.data[point_offset], &clouds[i]->data[cp * clouds[i]->point_step],
+          clouds[i]->point_step);
         point_offset += clouds[i]->point_step;
       }
     }
@@ -157,14 +161,13 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback (const PointClo
   }
 
   // Clean the queue to avoid overflowing
-  if (maximum_queue_size_ > 0)
-  {
-    while ((int)queue_.size () > maximum_queue_size_)
+  if (maximum_queue_size_ > 0) {
+    while ((int)queue_.size() > maximum_queue_size_) {
       queue_.erase(queue_.begin());
     }
+  }
 
 }
 
 typedef pcl_ros::PointCloudConcatenateFieldsSynchronizer PointCloudConcatenateFieldsSynchronizer;
 PLUGINLIB_EXPORT_CLASS(PointCloudConcatenateFieldsSynchronizer, nodelet::Nodelet);
-

pcl_ros/src/pcl_ros/io/io.cpp

@@ -42,7 +42,8 @@
 #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::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> NodeletDEMUX;
 
@@ -54,4 +55,3 @@ typedef nodelet::NodeletDEMUX<sensor_msgs::PointCloud2> NodeletDEMUX;
 PLUGINLIB_EXPORT_CLASS(NodeletMUX, nodelet::Nodelet);
 PLUGINLIB_EXPORT_CLASS(NodeletDEMUX, nodelet::Nodelet);
 //PLUGINLIB_EXPORT_CLASS(NodeletDEMUX_ROS,nodelet::Nodelet);
-

pcl_ros/src/pcl_ros/io/pcd_io.cpp

@@ -49,7 +49,8 @@ pcl_ros::PCDReader::onInit ()
   pnh_->getParam("publish_rate", publish_rate_);
   pnh_->getParam("tf_frame", tf_frame_);
 
-  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
+  NODELET_DEBUG(
+    "[%s::onInit] Nodelet successfully created with the following parameters:\n"
     " - publish_rate : %f\n"
     " - tf_frame     : %s",
     getName().c_str(),
@@ -60,35 +61,31 @@ pcl_ros::PCDReader::onInit ()
   output_new = boost::make_shared<PointCloud2>();
 
   // Wait in a loop until someone connects
-  do
-  {
+  do {
     ROS_DEBUG_ONCE("[%s::onInit] Waiting for a client to connect...", getName().c_str());
     ros::spinOnce();
     ros::Duration(0.01).sleep();
-  }
-  while (pnh_->ok () && pub_output.getNumSubscribers () == 0);
+  } while (pnh_->ok() && pub_output.getNumSubscribers() == 0);
 
   std::string file_name;
 
-  while (pnh_->ok ())
-  {
+  while (pnh_->ok()) {
     // Get the current filename parameter. If no filename set, loop
-    if (!pnh_->getParam ("filename", file_name_) && file_name_.empty ())
-    {
-      ROS_ERROR_ONCE ("[%s::onInit] Need a 'filename' parameter to be set before continuing!", getName ().c_str ());
+    if (!pnh_->getParam("filename", file_name_) && file_name_.empty()) {
+      ROS_ERROR_ONCE(
+        "[%s::onInit] Need a 'filename' parameter to be set before continuing!",
+        getName().c_str());
       ros::Duration(0.01).sleep();
       ros::spinOnce();
       continue;
     }
 
     // If the filename parameter holds a different value than the last one we read
-    if (file_name_.compare (file_name) != 0 && !file_name_.empty ())
-    {
+    if (file_name_.compare(file_name) != 0 && !file_name_.empty()) {
       NODELET_INFO("[%s::onInit] New file given: %s", getName().c_str(), file_name_.c_str());
       file_name = file_name_;
       pcl::PCLPointCloud2 cloud;
-      if (impl_.read (file_name_, cloud) < 0)
-      {
+      if (impl_.read(file_name_, cloud) < 0) {
         NODELET_ERROR("[%s::onInit] Error reading %s !", getName().c_str(), file_name_.c_str());
         return;
       }
@@ -98,22 +95,25 @@ pcl_ros::PCDReader::onInit ()
     }
 
     // We do not publish empty data
-    if (output_->data.size () == 0)
+    if (output_->data.size() == 0) {
       continue;
+    }
 
-    if (publish_rate_ == 0)
-    {
-      if (output_ != output_new)
-      {
-        NODELET_DEBUG ("Publishing data once (%d points) on topic %s in frame %s.", output_->width * output_->height, getMTPrivateNodeHandle ().resolveName ("output").c_str (), output_->header.frame_id.c_str ());
+    if (publish_rate_ == 0) {
+      if (output_ != output_new) {
+        NODELET_DEBUG(
+          "Publishing data once (%d points) on topic %s in frame %s.",
+          output_->width * output_->height,
+          getMTPrivateNodeHandle().resolveName("output").c_str(), output_->header.frame_id.c_str());
         pub_output.publish(output_);
         output_new = output_;
       }
       ros::Duration(0.01).sleep();
-    }
-    else
-    {
-      NODELET_DEBUG ("Publishing data (%d points) on topic %s in frame %s.", output_->width * output_->height, getMTPrivateNodeHandle ().resolveName ("output").c_str (), output_->header.frame_id.c_str ());
+    } else {
+      NODELET_DEBUG(
+        "Publishing data (%d points) on topic %s in frame %s.",
+        output_->width * output_->height, getMTPrivateNodeHandle().resolveName(
+          "output").c_str(), output_->header.frame_id.c_str());
       output_->header.stamp = ros::Time::now();
       pub_output.publish(output_);
 
@@ -142,7 +142,8 @@ pcl_ros::PCDWriter::onInit ()
   pnh_->getParam("filename", file_name_);
   pnh_->getParam("binary_mode", binary_mode_);
 
-  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
+  NODELET_DEBUG(
+    "[%s::onInit] Nodelet successfully created with the following parameters:\n"
     " - filename     : %s\n"
     " - binary_mode  : %s",
     getName().c_str(),
@@ -155,22 +156,29 @@ pcl_ros::PCDWriter::onInit ()
 void
 pcl_ros::PCDWriter::input_callback(const PointCloud2ConstPtr & cloud)
 {
-  if (!isValid (cloud))
+  if (!isValid(cloud)) {
     return;
+  }
 
   pnh_->getParam("filename", file_name_);
 
-  NODELET_DEBUG ("[%s::input_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 (), getMTPrivateNodeHandle ().resolveName ("input").c_str ());
+  NODELET_DEBUG(
+    "[%s::input_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(), getMTPrivateNodeHandle().resolveName("input").c_str());
 
   std::string fname;
-  if (file_name_.empty ())
+  if (file_name_.empty()) {
     fname = boost::lexical_cast<std::string>(cloud->header.stamp.toSec()) + ".pcd";
-  else
+  } else {
     fname = file_name_;
+  }
   pcl::PCLPointCloud2 pcl_cloud;
   // It is safe to remove the const here because we are the only subscriber callback.
   pcl_conversions::moveToPCL(*(const_cast<PointCloud2 *>(cloud.get())), pcl_cloud);
-  impl_.write (fname, pcl_cloud, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), binary_mode_);
+  impl_.write(
+    fname, pcl_cloud, Eigen::Vector4f::Zero(),
+    Eigen::Quaternionf::Identity(), binary_mode_);
 
   NODELET_DEBUG("[%s::input_callback] Data saved to %s", getName().c_str(), fname.c_str());
 }
@@ -179,4 +187,3 @@ typedef pcl_ros::PCDReader PCDReader;
 typedef pcl_ros::PCDWriter PCDWriter;
 PLUGINLIB_EXPORT_CLASS(PCDReader, nodelet::Nodelet);
 PLUGINLIB_EXPORT_CLASS(PCDWriter, nodelet::Nodelet);
-

pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp

@@ -55,32 +55,37 @@ pcl_ros::EuclideanClusterExtraction::onInit ()
 
   // ---[ Mandatory parameters
   double cluster_tolerance;
-  if (!pnh_->getParam ("cluster_tolerance", cluster_tolerance))
-  {
-    NODELET_ERROR ("[%s::onInit] Need a 'cluster_tolerance' parameter to be set before continuing!", getName ().c_str ()); 
+  if (!pnh_->getParam("cluster_tolerance", cluster_tolerance)) {
+    NODELET_ERROR(
+      "[%s::onInit] Need a 'cluster_tolerance' parameter to be set before continuing!",
+      getName().c_str());
     return;
   }
   int spatial_locator;
-  if (!pnh_->getParam ("spatial_locator", spatial_locator))
-  {
-    NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
+  if (!pnh_->getParam("spatial_locator", spatial_locator)) {
+    NODELET_ERROR(
+      "[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!",
+      getName().c_str());
     return;
   }
 
   //private_nh.getParam ("use_indices", use_indices_);
   pnh_->getParam("publish_indices", publish_indices_);
 
-  if (publish_indices_)
+  if (publish_indices_) {
     pub_output_ = advertise<PointIndices>(*pnh_, "output", max_queue_size_);
-  else
+  } else {
     pub_output_ = advertise<PointCloud>(*pnh_, "output", max_queue_size_);
+  }
 
   // Enable the dynamic reconfigure service
   srv_ = boost::make_shared<dynamic_reconfigure::Server<EuclideanClusterExtractionConfig>>(*pnh_);
-  dynamic_reconfigure::Server<EuclideanClusterExtractionConfig>::CallbackType f =  boost::bind (&EuclideanClusterExtraction::config_callback, this, _1, _2);
+  dynamic_reconfigure::Server<EuclideanClusterExtractionConfig>::CallbackType f = boost::bind(
+    &EuclideanClusterExtraction::config_callback, this, _1, _2);
   srv_->setCallback(f);
 
-  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
+  NODELET_DEBUG(
+    "[%s::onInit] Nodelet successfully created with the following parameters:\n"
     " - max_queue_size    : %d\n"
     " - use_indices       : %s\n"
     " - cluster_tolerance : %f\n",
@@ -99,66 +104,78 @@ void
 pcl_ros::EuclideanClusterExtraction::subscribe()
 {
   // If we're supposed to look for PointIndices (indices)
-  if (use_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_);
 
-    if (approximate_sync_)
-    {
-      sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloud, PointIndices> > > (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_->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_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_->connectInput(sub_input_filter_, sub_indices_filter_);
-      sync_input_indices_e_->registerCallback (bind (&EuclideanClusterExtraction::input_indices_callback, this, _1, _2));
+      sync_input_indices_e_->registerCallback(
+        bind(
+          &EuclideanClusterExtraction::
+          input_indices_callback, this, _1, _2));
     }
-  }
-  else
+  } else {
     // Subscribe in an old fashion to input only (no filters)
-    sub_input_ = pnh_->subscribe<PointCloud> ("input", max_queue_size_, bind (&EuclideanClusterExtraction::input_indices_callback, this, _1, PointIndicesConstPtr ()));
+    sub_input_ =
+      pnh_->subscribe<PointCloud>(
+      "input", max_queue_size_,
+      bind(&EuclideanClusterExtraction::input_indices_callback, this, _1, PointIndicesConstPtr()));
+  }
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
 pcl_ros::EuclideanClusterExtraction::unsubscribe()
 {
-  if (use_indices_)
-  {
+  if (use_indices_) {
     sub_input_filter_.unsubscribe();
     sub_indices_filter_.unsubscribe();
-  }
-  else
+  } else {
     sub_input_.shutdown();
   }
+}
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
-pcl_ros::EuclideanClusterExtraction::config_callback (EuclideanClusterExtractionConfig &config, uint32_t level)
-{
-  if (impl_.getClusterTolerance () != config.cluster_tolerance)
+pcl_ros::EuclideanClusterExtraction::config_callback(
+  EuclideanClusterExtractionConfig & config,
+  uint32_t level)
 {
+  if (impl_.getClusterTolerance() != config.cluster_tolerance) {
     impl_.setClusterTolerance(config.cluster_tolerance);
-    NODELET_DEBUG ("[%s::config_callback] Setting new clustering tolerance to: %f.", getName ().c_str (), config.cluster_tolerance);
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting new clustering tolerance to: %f.",
+      getName().c_str(), config.cluster_tolerance);
   }
-  if (impl_.getMinClusterSize () != config.cluster_min_size)
-  {
+  if (impl_.getMinClusterSize() != config.cluster_min_size) {
     impl_.setMinClusterSize(config.cluster_min_size);
-    NODELET_DEBUG ("[%s::config_callback] Setting the minimum cluster size to: %d.", getName ().c_str (), config.cluster_min_size);
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting the minimum cluster size to: %d.",
+      getName().c_str(), config.cluster_min_size);
   }
-  if (impl_.getMaxClusterSize () != config.cluster_max_size)
-  {
+  if (impl_.getMaxClusterSize() != config.cluster_max_size) {
     impl_.setMaxClusterSize(config.cluster_max_size);
-    NODELET_DEBUG ("[%s::config_callback] Setting the maximum cluster size to: %d.", getName ().c_str (), config.cluster_max_size);
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting the maximum cluster size to: %d.",
+      getName().c_str(), config.cluster_max_size);
   }
-  if (max_clusters_ != config.max_clusters)
-  {
+  if (max_clusters_ != config.max_clusters) {
     max_clusters_ = config.max_clusters;
-    NODELET_DEBUG ("[%s::config_callback] Setting the maximum number of clusters to extract to: %d.", getName ().c_str (), config.max_clusters);
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting the maximum number of clusters to extract to: %d.",
+      getName().c_str(), config.max_clusters);
   }
 }
 
@@ -168,18 +185,17 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback (
   const PointCloudConstPtr & cloud, const PointIndicesConstPtr & indices)
 {
   // No subscribers, no work
-  if (pub_output_.getNumSubscribers () <= 0)
+  if (pub_output_.getNumSubscribers() <= 0) {
     return;
+  }
 
   // If cloud is given, check if it's valid
-  if (!isValid (cloud))
-  {
+  if (!isValid(cloud)) {
     NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str());
     return;
   }
   // If indices are given, check if they are valid
-  if (indices && !isValid (indices))
-  {
+  if (indices && !isValid(indices)) {
     NODELET_ERROR("[%s::input_indices_callback] Invalid indices!", getName().c_str());
     return;
   }
@@ -188,20 +204,28 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback (
   if (indices) {
     std_msgs::Header cloud_header = fromPCL(cloud->header);
     std_msgs::Header indices_header = indices->header;
-    NODELET_DEBUG ("[%s::input_indices_callback]\n"
+    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(),
-                   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->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());
   } else {
-    NODELET_DEBUG ("[%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 ());
+    NODELET_DEBUG(
+      "[%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());
   }
   ///
 
   IndicesPtr indices_ptr;
-  if (indices)
+  if (indices) {
     indices_ptr.reset(new std::vector<int>(indices->indices));
+  }
 
   impl_.setInputCloud(pcl_ptr(cloud));
   impl_.setIndices(indices_ptr);
@@ -209,12 +233,11 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback (
   std::vector<pcl::PointIndices> clusters;
   impl_.extract(clusters);
 
-  if (publish_indices_)
-  {
-    for (size_t i = 0; i < clusters.size (); ++i)
-    {
-      if ((int)i >= max_clusters_)
+  if (publish_indices_) {
+    for (size_t i = 0; i < clusters.size(); ++i) {
+      if ((int)i >= max_clusters_) {
         break;
+      }
       // TODO: 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);
@@ -222,14 +245,14 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback (
       pub_output_.publish(ros_pi);
     }
 
-    NODELET_DEBUG ("[segmentAndPublish] Published %zu clusters (PointIndices) on topic %s", clusters.size (), pnh_->resolveName ("output").c_str ());
-  }
-  else
-  {
-    for (size_t i = 0; i < clusters.size (); ++i)
-    {
-      if ((int)i >= max_clusters_)
+    NODELET_DEBUG(
+      "[segmentAndPublish] Published %zu clusters (PointIndices) on topic %s",
+      clusters.size(), pnh_->resolveName("output").c_str());
+  } else {
+    for (size_t i = 0; i < clusters.size(); ++i) {
+      if ((int)i >= max_clusters_) {
         break;
+      }
       PointCloud output;
       copyPointCloud(*cloud, clusters[i].indices, output);
 
@@ -241,7 +264,8 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback (
       toPCL(header, output.header);
       // Publish a Boost shared ptr const data
       pub_output_.publish(ros_ptr(output.makeShared()));
-      NODELET_DEBUG ("[segmentAndPublish] Published cluster %zu (with %zu values and stamp %f) on topic %s",
+      NODELET_DEBUG(
+        "[segmentAndPublish] Published cluster %zu (with %zu values and stamp %f) on topic %s",
         i, clusters[i].indices.size(), header.stamp.toSec(), pnh_->resolveName("output").c_str());
     }
   }
@@ -249,4 +273,3 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback (
 
 typedef pcl_ros::EuclideanClusterExtraction EuclideanClusterExtraction;
 PLUGINLIB_EXPORT_CLASS(EuclideanClusterExtraction, nodelet::Nodelet)
-

pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp

@@ -54,7 +54,8 @@ pcl_ros::ExtractPolygonalPrismData::onInit ()
 
   // Enable the dynamic reconfigure service
   srv_ = boost::make_shared<dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig>>(*pnh_);
-  dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig>::CallbackType f = boost::bind (&ExtractPolygonalPrismData::config_callback, this, _1, _2);
+  dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig>::CallbackType f = boost::bind(
+    &ExtractPolygonalPrismData::config_callback, this, _1, _2);
   srv_->setCallback(f);
 
   // Advertise the output topics
@@ -71,33 +72,46 @@ pcl_ros::ExtractPolygonalPrismData::subscribe ()
   sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_);
 
   // 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_);
-  else
-    sync_input_hull_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloud, PointIndices> > > (max_queue_size_);
-
-  if (use_indices_)
-  {
-    sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
-    if (approximate_sync_)
-      sync_input_hull_indices_a_->connectInput (sub_input_filter_, sub_hull_filter_, sub_indices_filter_);
-    else
-      sync_input_hull_indices_e_->connectInput (sub_input_filter_, sub_hull_filter_, sub_indices_filter_);
+  if (approximate_sync_) {
+    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_);
   }
-  else
-  {
+
+  if (use_indices_) {
+    sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
+    if (approximate_sync_) {
+      sync_input_hull_indices_a_->connectInput(
+        sub_input_filter_, sub_hull_filter_,
+        sub_indices_filter_);
+    } else {
+      sync_input_hull_indices_e_->connectInput(
+        sub_input_filter_, sub_hull_filter_,
+        sub_indices_filter_);
+    }
+  } else {
     sub_input_filter_.registerCallback(bind(&ExtractPolygonalPrismData::input_callback, this, _1));
 
-    if (approximate_sync_)
+    if (approximate_sync_) {
       sync_input_hull_indices_a_->connectInput(sub_input_filter_, sub_hull_filter_, nf_);
-    else
+    } else {
       sync_input_hull_indices_e_->connectInput(sub_input_filter_, sub_hull_filter_, nf_);
     }
+  }
   // Register callbacks
-  if (approximate_sync_)
-    sync_input_hull_indices_a_->registerCallback (bind (&ExtractPolygonalPrismData::input_hull_indices_callback, this, _1, _2, _3));
-  else
-    sync_input_hull_indices_e_->registerCallback (bind (&ExtractPolygonalPrismData::input_hull_indices_callback, this, _1, _2, _3));
+  if (approximate_sync_) {
+    sync_input_hull_indices_a_->registerCallback(
+      bind(
+        &ExtractPolygonalPrismData::
+        input_hull_indices_callback, this, _1, _2, _3));
+  } else {
+    sync_input_hull_indices_e_->registerCallback(
+      bind(
+        &ExtractPolygonalPrismData::
+        input_hull_indices_callback, this, _1, _2, _3));
+  }
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
@@ -107,26 +121,31 @@ pcl_ros::ExtractPolygonalPrismData::unsubscribe ()
   sub_hull_filter_.unsubscribe();
   sub_input_filter_.unsubscribe();
 
-  if (use_indices_)
+  if (use_indices_) {
     sub_indices_filter_.unsubscribe();
   }
+}
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
-pcl_ros::ExtractPolygonalPrismData::config_callback (ExtractPolygonalPrismDataConfig &config, uint32_t level)
+pcl_ros::ExtractPolygonalPrismData::config_callback(
+  ExtractPolygonalPrismDataConfig & config,
+  uint32_t level)
 {
   double height_min, height_max;
   impl_.getHeightLimits(height_min, height_max);
-  if (height_min != config.height_min)
-  {
+  if (height_min != config.height_min) {
     height_min = config.height_min;
-    NODELET_DEBUG ("[%s::config_callback] Setting new minimum height to the planar model to: %f.", getName ().c_str (), height_min);
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting new minimum height to the planar model to: %f.",
+      getName().c_str(), height_min);
     impl_.setHeightLimits(height_min, height_max);
   }
-  if (height_max != config.height_max)
-  {
+  if (height_max != config.height_max) {
     height_max = config.height_max;
-    NODELET_DEBUG ("[%s::config_callback] Setting new maximum height to the planar model to: %f.", getName ().c_str (), height_max);
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting new maximum height to the planar model to: %f.",
+      getName().c_str(), height_max);
     impl_.setHeightLimits(height_min, height_max);
   }
 }
@@ -139,65 +158,77 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback (
   const PointIndicesConstPtr & indices)
 {
   // No subscribers, no work
-  if (pub_output_.getNumSubscribers () <= 0)
+  if (pub_output_.getNumSubscribers() <= 0) {
     return;
+  }
 
   // Copy the header (stamp + frame_id)
   pcl_msgs::PointIndices inliers;
   inliers.header = fromPCL(cloud->header);
 
   // If cloud is given, check if it's valid
-  if (!isValid (cloud) || !isValid (hull, "planar_hull"))
-  {
+  if (!isValid(cloud) || !isValid(hull, "planar_hull")) {
     NODELET_ERROR("[%s::input_hull_indices_callback] Invalid input!", getName().c_str());
     pub_output_.publish(inliers);
     return;
   }
   // If indices are given, check if they are valid
-  if (indices && !isValid (indices))
-  {
+  if (indices && !isValid(indices)) {
     NODELET_ERROR("[%s::input_hull_indices_callback] Invalid indices!", getName().c_str());
     pub_output_.publish(inliers);
     return;
   }
 
   /// DEBUG
-  if (indices)
-    NODELET_DEBUG ("[%s::input_indices_hull_callback]\n"
+  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.",
       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 ("input").c_str (),
-                   hull->width * hull->height, pcl::getFieldsList (*hull).c_str (), fromPCL(hull->header).stamp.toSec (), hull->header.frame_id.c_str (), pnh_->resolveName ("planar_hull").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_indices_hull_callback]\n"
+      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(),
+      hull->width * hull->height, pcl::getFieldsList(*hull).c_str(), fromPCL(
+        hull->header).stamp.toSec(), hull->header.frame_id.c_str(), pnh_->resolveName(
+        "planar_hull").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_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.",
       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 ("input").c_str (),
-                   hull->width * hull->height, pcl::getFieldsList (*hull).c_str (), fromPCL(hull->header).stamp.toSec (), hull->header.frame_id.c_str (), pnh_->resolveName ("planar_hull").c_str ());
+      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(),
+      hull->width * hull->height, pcl::getFieldsList(*hull).c_str(), fromPCL(
+        hull->header).stamp.toSec(), hull->header.frame_id.c_str(), pnh_->resolveName(
+        "planar_hull").c_str());
+  }
   ///
 
-  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.", getName ().c_str (), hull->header.frame_id.c_str (), cloud->header.frame_id.c_str ());
+  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.",
+      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_))
-    {
+    if (!pcl_ros::transformPointCloud(cloud->header.frame_id, *hull, planar_hull, tf_listener_)) {
       // Publish empty before return
       pub_output_.publish(inliers);
       return;
     }
     impl_.setInputPlanarHull(pcl_ptr(planar_hull.makeShared()));
-  }
-  else
+  } else {
     impl_.setInputPlanarHull(pcl_ptr(hull));
+  }
 
   IndicesPtr indices_ptr;
-  if (indices && !indices->header.frame_id.empty ())
+  if (indices && !indices->header.frame_id.empty()) {
     indices_ptr.reset(new std::vector<int>(indices->indices));
+  }
 
   impl_.setInputCloud(pcl_ptr(cloud));
   impl_.setIndices(indices_ptr);
@@ -212,9 +243,10 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback (
   // Enforce that the TF frame and the timestamp are copied
   inliers.header = fromPCL(cloud->header);
   pub_output_.publish(inliers);
-  NODELET_DEBUG ("[%s::input_hull_callback] Publishing %zu indices.", getName ().c_str (), inliers.indices.size ());
+  NODELET_DEBUG(
+    "[%s::input_hull_callback] Publishing %zu indices.",
+    getName().c_str(), inliers.indices.size());
 }
 
 typedef pcl_ros::ExtractPolygonalPrismData ExtractPolygonalPrismData;
 PLUGINLIB_EXPORT_CLASS(ExtractPolygonalPrismData, nodelet::Nodelet)
-

pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp

@@ -57,14 +57,12 @@ pcl_ros::SACSegmentation::onInit ()
 
   // ---[ Mandatory parameters
   int model_type;
-  if (!pnh_->getParam ("model_type", model_type))
-  {
+  if (!pnh_->getParam("model_type", model_type)) {
     NODELET_ERROR("[onInit] Need a 'model_type' parameter to be set before continuing!");
     return;
   }
   double threshold; // unused - set via dynamic reconfigure in the callback
-  if (!pnh_->getParam ("distance_threshold", threshold))
-  {
+  if (!pnh_->getParam("distance_threshold", threshold)) {
     NODELET_ERROR("[onInit] Need a 'distance_threshold' parameter to be set before continuing!");
     return;
   }
@@ -77,20 +75,20 @@ pcl_ros::SACSegmentation::onInit ()
   pnh_->getParam("axis", axis_param);
   Eigen::Vector3f axis = Eigen::Vector3f::Zero();
 
-  switch (axis_param.getType ())
-  {
+  switch (axis_param.getType()) {
     case XmlRpc::XmlRpcValue::TypeArray:
       {
-      if (axis_param.size () != 3)
-      {
-        NODELET_ERROR ("[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than required (3)!", getName ().c_str (), axis_param.size ());
+        if (axis_param.size() != 3) {
+          NODELET_ERROR(
+            "[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than required (3)!",
+            getName().c_str(), axis_param.size());
           return;
         }
-      for (int i = 0; i < 3; ++i)
-      {
-        if (axis_param[i].getType () != XmlRpc::XmlRpcValue::TypeDouble)
-        {
-          NODELET_ERROR ("[%s::onInit] Need floating point values for 'axis' parameter.", getName ().c_str ());
+        for (int i = 0; i < 3; ++i) {
+          if (axis_param[i].getType() != XmlRpc::XmlRpcValue::TypeDouble) {
+            NODELET_ERROR(
+              "[%s::onInit] Need floating point values for 'axis' parameter.",
+              getName().c_str());
             return;
           }
           double value = axis_param[i]; axis[i] = value;
@@ -108,10 +106,12 @@ pcl_ros::SACSegmentation::onInit ()
 
   // Enable the dynamic reconfigure service
   srv_ = boost::make_shared<dynamic_reconfigure::Server<SACSegmentationConfig>>(*pnh_);
-  dynamic_reconfigure::Server<SACSegmentationConfig>::CallbackType f =  boost::bind (&SACSegmentation::config_callback, this, _1, _2);
+  dynamic_reconfigure::Server<SACSegmentationConfig>::CallbackType f = boost::bind(
+    &SACSegmentation::config_callback, this, _1, _2);
   srv_->setCallback(f);
 
-  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
+  NODELET_DEBUG(
+    "[%s::onInit] Nodelet successfully created with the following parameters:\n"
     " - model_type               : %d\n"
     " - method_type              : %d\n"
     " - model_threshold          : %f\n"
@@ -132,8 +132,7 @@ void
 pcl_ros::SACSegmentation::subscribe()
 {
   // If we're supposed to look for PointIndices (indices)
-  if (use_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_);
@@ -142,8 +141,7 @@ pcl_ros::SACSegmentation::subscribe ()
     // we'll subscribe and get a separate callback for PointIndices that will
     // save the indices internally, and a PointCloud + PointIndices callback
     // will take care of meshing the new PointClouds with the old saved indices.
-    if (latched_indices_)
-    {
+    if (latched_indices_) {
       // Subscribe to a callback that saves the indices
       sub_indices_filter_.registerCallback(bind(&SACSegmentation::indices_callback, this, _1));
       // Subscribe to a callback that sets the header of the saved indices to the cloud header
@@ -151,44 +149,54 @@ 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));
+      sync_input_indices_e_->registerCallback(
+        bind(
+          &SACSegmentation::input_indices_callback, this,
+          _1, _2));
     }
     // "latched_indices" not set, proceed with regular <input,indices> pairs
-    else
-    {
-      if (approximate_sync_)
-      {
-        sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > > (max_queue_size_);
+    else {
+      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_->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_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_->connectInput(sub_input_filter_, sub_indices_filter_);
-        sync_input_indices_e_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2));
+        sync_input_indices_e_->registerCallback(
+          bind(
+            &SACSegmentation::input_indices_callback, this,
+            _1, _2));
       }
     }
-  }
-  else
+  } else {
     // Subscribe in an old fashion to input only (no filters)
-    sub_input_ = pnh_->subscribe<PointCloud> ("input", max_queue_size_,  bind (&SACSegmentation::input_indices_callback, this, _1, PointIndicesConstPtr ()));
+    sub_input_ =
+      pnh_->subscribe<PointCloud>(
+      "input", max_queue_size_,
+      bind(&SACSegmentation::input_indices_callback, this, _1, PointIndicesConstPtr()));
+  }
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
 pcl_ros::SACSegmentation::unsubscribe()
 {
-  if (use_indices_)
-  {
+  if (use_indices_) {
     sub_input_filter_.unsubscribe();
     sub_indices_filter_.unsubscribe();
-  }
-  else
+  } else {
     sub_input_.shutdown();
   }
+}
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
@@ -196,76 +204,81 @@ pcl_ros::SACSegmentation::config_callback (SACSegmentationConfig &config, uint32
 {
   boost::mutex::scoped_lock lock(mutex_);
 
-  if (impl_.getDistanceThreshold () != config.distance_threshold)
-  {
+  if (impl_.getDistanceThreshold() != config.distance_threshold) {
     //sac_->setDistanceThreshold (threshold_); - done in initSAC
     impl_.setDistanceThreshold(config.distance_threshold);
-    NODELET_DEBUG ("[%s::config_callback] Setting new distance to model threshold to: %f.", getName ().c_str (), config.distance_threshold);
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting new distance to model threshold to: %f.",
+      getName().c_str(), config.distance_threshold);
   }
   // The maximum allowed difference between the model normal and the given axis _in radians_
-  if (impl_.getEpsAngle () != config.eps_angle)
-  {
+  if (impl_.getEpsAngle() != config.eps_angle) {
     impl_.setEpsAngle(config.eps_angle);
-    NODELET_DEBUG ("[%s::config_callback] Setting new epsilon angle to model threshold to: %f (%f degrees).", getName ().c_str (), config.eps_angle, config.eps_angle * 180.0 / M_PI);
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting new epsilon angle to model threshold to: %f (%f degrees).",
+      getName().c_str(), config.eps_angle, config.eps_angle * 180.0 / M_PI);
   }
 
   // Number of inliers
-  if (min_inliers_ != config.min_inliers)
-  {
+  if (min_inliers_ != config.min_inliers) {
     min_inliers_ = config.min_inliers;
-    NODELET_DEBUG ("[%s::config_callback] Setting new minimum number of inliers to: %d.",  getName ().c_str (), min_inliers_);
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting new minimum number of inliers to: %d.",
+      getName().c_str(), min_inliers_);
   }
 
-  if (impl_.getMaxIterations () != config.max_iterations)
-  {
+  if (impl_.getMaxIterations() != config.max_iterations) {
     //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);
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting new maximum number of iterations to: %d.",
+      getName().c_str(), config.max_iterations);
   }
-  if (impl_.getProbability () != config.probability)
-  {
+  if (impl_.getProbability() != config.probability) {
     //sac_->setProbability (probability_); - done in initSAC
     impl_.setProbability(config.probability);
-    NODELET_DEBUG ("[%s::config_callback] Setting new probability to: %f.", getName ().c_str (), config.probability);
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting new probability to: %f.",
+      getName().c_str(), config.probability);
   }
-  if (impl_.getOptimizeCoefficients () != config.optimize_coefficients)
-  {
+  if (impl_.getOptimizeCoefficients() != config.optimize_coefficients) {
     impl_.setOptimizeCoefficients(config.optimize_coefficients);
-    NODELET_DEBUG ("[%s::config_callback] Setting coefficient optimization to: %s.", getName ().c_str (), (config.optimize_coefficients) ? "true" : "false");
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting coefficient optimization to: %s.",
+      getName().c_str(), (config.optimize_coefficients) ? "true" : "false");
   }
 
   double radius_min, radius_max;
   impl_.getRadiusLimits(radius_min, radius_max);
-  if (radius_min != config.radius_min)
-  {
+  if (radius_min != config.radius_min) {
     radius_min = config.radius_min;
     NODELET_DEBUG("[config_callback] Setting minimum allowable model radius to: %f.", radius_min);
     impl_.setRadiusLimits(radius_min, radius_max);
   }
-  if (radius_max != config.radius_max)
-  {
+  if (radius_max != config.radius_max) {
     radius_max = config.radius_max;
     NODELET_DEBUG("[config_callback] Setting maximum allowable model radius to: %f.", radius_max);
     impl_.setRadiusLimits(radius_min, radius_max);
   }
 
-  if (tf_input_frame_ != config.input_frame)
-  {
+  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());
     NODELET_WARN("input_frame TF not implemented yet!");
   }
-  if (tf_output_frame_ != config.output_frame)
-  {
+  if (tf_output_frame_ != config.output_frame) {
     tf_output_frame_ = config.output_frame;
-    NODELET_DEBUG ("[config_callback] Setting the output TF frame to: %s.", tf_output_frame_.c_str ());
+    NODELET_DEBUG(
+      "[config_callback] Setting the output TF frame to: %s.",
+      tf_output_frame_.c_str());
     NODELET_WARN("output_frame TF not implemented yet!");
   }
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
-pcl_ros::SACSegmentation::input_indices_callback (const PointCloudConstPtr &cloud, 
+pcl_ros::SACSegmentation::input_indices_callback(
+  const PointCloudConstPtr & cloud,
   const PointIndicesConstPtr & indices)
 {
   boost::mutex::scoped_lock lock(mutex_);
@@ -276,16 +289,14 @@ pcl_ros::SACSegmentation::input_indices_callback (const PointCloudConstPtr &clou
   inliers.header = model.header = fromPCL(cloud->header);
 
   // If cloud is given, check if it's valid
-  if (!isValid (cloud)) 
-  {
+  if (!isValid(cloud)) {
     NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str());
     pub_indices_.publish(inliers);
     pub_model_.publish(model);
     return;
   }
   // If indices are given, check if they are valid
-  if (indices && !isValid (indices))
-  {
+  if (indices && !isValid(indices)) {
     NODELET_ERROR("[%s::input_indices_callback] Invalid indices!", getName().c_str());
     pub_indices_.publish(inliers);
     pub_model_.publish(model);
@@ -293,16 +304,24 @@ pcl_ros::SACSegmentation::input_indices_callback (const PointCloudConstPtr &clou
   }
 
   /// DEBUG
-  if (indices && !indices->header.frame_id.empty ())
-    NODELET_DEBUG ("[%s::input_indices_callback]\n"
+  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.",
       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 ("input").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_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 ());
+      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(),
+      indices->indices.size(), indices->header.stamp.toSec(),
+      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.",
+      getName().c_str(), cloud->width * cloud->height, fromPCL(
+        cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
+        "input").c_str());
+  }
   ///
 
   // Check whether the user has given a different input TF frame
@@ -322,8 +341,9 @@ pcl_ros::SACSegmentation::input_indices_callback (const PointCloudConstPtr &clou
   cloud_tf = cloud;
 
   IndicesPtr indices_ptr;
-  if (indices && !indices->header.frame_id.empty ())
+  if (indices && !indices->header.frame_id.empty()) {
     indices_ptr.reset(new std::vector<int>(indices->indices));
+  }
 
   impl_.setInputCloud(pcl_ptr(cloud_tf));
   impl_.setIndices(indices_ptr);
@@ -342,8 +362,7 @@ pcl_ros::SACSegmentation::input_indices_callback (const PointCloudConstPtr &clou
   // 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 ((int)inliers.indices.size() <= min_inliers_) {
     inliers.indices.clear();
     model.values.clear();
   }
@@ -351,13 +370,15 @@ pcl_ros::SACSegmentation::input_indices_callback (const PointCloudConstPtr &clou
   // Publish
   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",
+  NODELET_DEBUG(
+    "[%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());
 
-  if (inliers.indices.empty ())
+  if (inliers.indices.empty()) {
     NODELET_WARN("[%s::input_indices_callback] No inliers found!", getName().c_str());
   }
+}
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
@@ -368,7 +389,8 @@ pcl_ros::SACSegmentationFromNormals::onInit ()
 
   // Enable the dynamic reconfigure service
   srv_ = boost::make_shared<dynamic_reconfigure::Server<SACSegmentationFromNormalsConfig>>(*pnh_);
-  dynamic_reconfigure::Server<SACSegmentationFromNormalsConfig>::CallbackType f = boost::bind (&SACSegmentationFromNormals::config_callback, this, _1, _2);
+  dynamic_reconfigure::Server<SACSegmentationFromNormalsConfig>::CallbackType f = boost::bind(
+    &SACSegmentationFromNormals::config_callback, this, _1, _2);
   srv_->setCallback(f);
 
   // Advertise the output topics
@@ -377,15 +399,17 @@ pcl_ros::SACSegmentationFromNormals::onInit ()
 
   // ---[ Mandatory parameters
   int model_type;
-  if (!pnh_->getParam ("model_type", model_type))
-  {
-    NODELET_ERROR ("[%s::onInit] Need a 'model_type' parameter to be set before continuing!", getName ().c_str ());
+  if (!pnh_->getParam("model_type", model_type)) {
+    NODELET_ERROR(
+      "[%s::onInit] Need a 'model_type' parameter to be set before continuing!",
+      getName().c_str());
     return;
   }
   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!", getName ().c_str ());
+  if (!pnh_->getParam("distance_threshold", threshold)) {
+    NODELET_ERROR(
+      "[%s::onInit] Need a 'distance_threshold' parameter to be set before continuing!",
+      getName().c_str());
     return;
   }
 
@@ -397,20 +421,20 @@ pcl_ros::SACSegmentationFromNormals::onInit ()
   pnh_->getParam("axis", axis_param);
   Eigen::Vector3f axis = Eigen::Vector3f::Zero();
 
-  switch (axis_param.getType ())
-  {
+  switch (axis_param.getType()) {
     case XmlRpc::XmlRpcValue::TypeArray:
       {
-      if (axis_param.size () != 3)
-      {
-        NODELET_ERROR ("[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than required (3)!", getName ().c_str (), axis_param.size ());
+        if (axis_param.size() != 3) {
+          NODELET_ERROR(
+            "[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than required (3)!",
+            getName().c_str(), axis_param.size());
           return;
         }
-      for (int i = 0; i < 3; ++i)
-      {
-        if (axis_param[i].getType () != XmlRpc::XmlRpcValue::TypeDouble)
-        {
-          NODELET_ERROR ("[%s::onInit] Need floating point values for 'axis' parameter.", getName ().c_str ());
+        for (int i = 0; i < 3; ++i) {
+          if (axis_param[i].getType() != XmlRpc::XmlRpcValue::TypeDouble) {
+            NODELET_ERROR(
+              "[%s::onInit] Need floating point values for 'axis' parameter.",
+              getName().c_str());
             return;
           }
           double value = axis_param[i]; axis[i] = value;
@@ -426,7 +450,8 @@ pcl_ros::SACSegmentationFromNormals::onInit ()
   // Initialize the random number generator
   srand(time(0));
 
-  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
+  NODELET_DEBUG(
+    "[%s::onInit] Nodelet successfully created with the following parameters:\n"
     " - model_type               : %d\n"
     " - method_type              : %d\n"
     " - model_threshold          : %f\n"
@@ -453,37 +478,50 @@ pcl_ros::SACSegmentationFromNormals::subscribe ()
   // 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_);
-  else
-    sync_input_normals_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloudN, PointIndices> > > (max_queue_size_);
+  if (approximate_sync_) {
+    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_);
+  }
 
   // If we're supposed to look for PointIndices (indices)
-  if (use_indices_)
-  {
+  if (use_indices_) {
     // Subscribe to the input using a filter
     sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
 
-    if (approximate_sync_)
-      sync_input_normals_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_indices_filter_);
-    else
-      sync_input_normals_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_indices_filter_);
+    if (approximate_sync_) {
+      sync_input_normals_indices_a_->connectInput(
+        sub_input_filter_, sub_normals_filter_,
+        sub_indices_filter_);
+    } else {
+      sync_input_normals_indices_e_->connectInput(
+        sub_input_filter_, sub_normals_filter_,
+        sub_indices_filter_);
     }
-  else
-  {
+  } else {
     // Create a different callback for copying over the timestamp to fake indices
     sub_input_filter_.registerCallback(bind(&SACSegmentationFromNormals::input_callback, this, _1));
 
-    if (approximate_sync_)
+    if (approximate_sync_) {
       sync_input_normals_indices_a_->connectInput(sub_input_filter_, sub_normals_filter_, nf_);
-    else
+    } else {
       sync_input_normals_indices_e_->connectInput(sub_input_filter_, sub_normals_filter_, nf_);
     }
+  }
 
-  if (approximate_sync_)
-    sync_input_normals_indices_a_->registerCallback (bind (&SACSegmentationFromNormals::input_normals_indices_callback, this, _1, _2, _3));
-  else
-    sync_input_normals_indices_e_->registerCallback (bind (&SACSegmentationFromNormals::input_normals_indices_callback, this, _1, _2, _3));
+  if (approximate_sync_) {
+    sync_input_normals_indices_a_->registerCallback(
+      bind(
+        &SACSegmentationFromNormals::
+        input_normals_indices_callback, this, _1, _2, _3));
+  } else {
+    sync_input_normals_indices_e_->registerCallback(
+      bind(
+        &SACSegmentationFromNormals::
+        input_normals_indices_callback, this, _1, _2, _3));
+  }
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
@@ -495,22 +533,27 @@ pcl_ros::SACSegmentationFromNormals::unsubscribe ()
 
   sub_axis_.shutdown();
 
-  if (use_indices_)
+  if (use_indices_) {
     sub_indices_filter_.unsubscribe();
   }
+}
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
-pcl_ros::SACSegmentationFromNormals::axis_callback (const pcl_msgs::ModelCoefficientsConstPtr &model)
+pcl_ros::SACSegmentationFromNormals::axis_callback(
+  const pcl_msgs::ModelCoefficientsConstPtr & model)
 {
   boost::mutex::scoped_lock lock(mutex_);
 
-  if (model->values.size () < 3)
-  {
-    NODELET_ERROR ("[%s::axis_callback] Invalid axis direction / model coefficients with %zu values sent on %s!", getName ().c_str (), model->values.size (), pnh_->resolveName ("axis").c_str ());
+  if (model->values.size() < 3) {
+    NODELET_ERROR(
+      "[%s::axis_callback] Invalid axis direction / model coefficients with %zu values sent on %s!",
+      getName().c_str(), model->values.size(), pnh_->resolveName("axis").c_str());
     return;
   }
-  NODELET_DEBUG ("[%s::axis_callback] Received axis direction: %f %f %f", getName ().c_str (), model->values[0], model->values[1], model->values[2]);
+  NODELET_DEBUG(
+    "[%s::axis_callback] Received axis direction: %f %f %f",
+    getName().c_str(), model->values[0], model->values[1], model->values[2]);
 
   Eigen::Vector3f axis(model->values[0], model->values[1], model->values[2]);
   impl_.setAxis(axis);
@@ -518,66 +561,77 @@ pcl_ros::SACSegmentationFromNormals::axis_callback (const pcl_msgs::ModelCoeffic
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
-pcl_ros::SACSegmentationFromNormals::config_callback (SACSegmentationFromNormalsConfig &config, uint32_t level)
+pcl_ros::SACSegmentationFromNormals::config_callback(
+  SACSegmentationFromNormalsConfig & config,
+  uint32_t level)
 {
   boost::mutex::scoped_lock lock(mutex_);
 
-  if (impl_.getDistanceThreshold () != config.distance_threshold)
-  {
+  if (impl_.getDistanceThreshold() != config.distance_threshold) {
     impl_.setDistanceThreshold(config.distance_threshold);
-    NODELET_DEBUG ("[%s::config_callback] Setting distance to model threshold to: %f.", getName ().c_str (), config.distance_threshold);
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting distance to model threshold to: %f.",
+      getName().c_str(), config.distance_threshold);
   }
   // The maximum allowed difference between the model normal and the given axis _in radians_
-  if (impl_.getEpsAngle () != config.eps_angle)
-  {
+  if (impl_.getEpsAngle() != config.eps_angle) {
     impl_.setEpsAngle(config.eps_angle);
-    NODELET_DEBUG ("[%s::config_callback] Setting new epsilon angle to model threshold to: %f (%f degrees).", getName ().c_str (), config.eps_angle, config.eps_angle * 180.0 / M_PI);
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting new epsilon angle to model threshold to: %f (%f degrees).",
+      getName().c_str(), config.eps_angle, config.eps_angle * 180.0 / M_PI);
   }
 
-  if (impl_.getMaxIterations () != config.max_iterations)
-  {
+  if (impl_.getMaxIterations() != config.max_iterations) {
     impl_.setMaxIterations(config.max_iterations);
-    NODELET_DEBUG ("[%s::config_callback] Setting new maximum number of iterations to: %d.", getName ().c_str (), config.max_iterations);
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting new maximum number of iterations to: %d.",
+      getName().c_str(), config.max_iterations);
   }
 
   // Number of inliers
-  if (min_inliers_ != config.min_inliers)
-  {
+  if (min_inliers_ != config.min_inliers) {
     min_inliers_ = config.min_inliers;
-    NODELET_DEBUG ("[%s::config_callback] Setting new minimum number of inliers to: %d.",  getName ().c_str (), min_inliers_);
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting new minimum number of inliers to: %d.",
+      getName().c_str(), min_inliers_);
   }
 
 
-  if (impl_.getProbability () != config.probability)
-  {
+  if (impl_.getProbability() != config.probability) {
     impl_.setProbability(config.probability);
-    NODELET_DEBUG ("[%s::config_callback] Setting new probability to: %f.", getName ().c_str (), config.probability);
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting new probability to: %f.",
+      getName().c_str(), config.probability);
   }
 
-  if (impl_.getOptimizeCoefficients () != config.optimize_coefficients)
-  {
+  if (impl_.getOptimizeCoefficients() != config.optimize_coefficients) {
     impl_.setOptimizeCoefficients(config.optimize_coefficients);
-    NODELET_DEBUG ("[%s::config_callback] Setting coefficient optimization to: %s.", getName ().c_str (), (config.optimize_coefficients) ? "true" : "false");
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting coefficient optimization to: %s.",
+      getName().c_str(), (config.optimize_coefficients) ? "true" : "false");
   }
 
-  if (impl_.getNormalDistanceWeight () != config.normal_distance_weight)
-  {
+  if (impl_.getNormalDistanceWeight() != config.normal_distance_weight) {
     impl_.setNormalDistanceWeight(config.normal_distance_weight);
-    NODELET_DEBUG ("[%s::config_callback] Setting new distance weight to: %f.", getName ().c_str (), config.normal_distance_weight);
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting new distance weight to: %f.",
+      getName().c_str(), config.normal_distance_weight);
   }
 
   double radius_min, radius_max;
   impl_.getRadiusLimits(radius_min, radius_max);
-  if (radius_min != config.radius_min)
-  {
+  if (radius_min != config.radius_min) {
     radius_min = config.radius_min;
-    NODELET_DEBUG ("[%s::config_callback] Setting minimum allowable model radius to: %f.", getName ().c_str (), radius_min);
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting minimum allowable model radius to: %f.",
+      getName().c_str(), radius_min);
     impl_.setRadiusLimits(radius_min, radius_max);
   }
-  if (radius_max != config.radius_max)
-  {
+  if (radius_max != config.radius_max) {
     radius_max = config.radius_max;
-    NODELET_DEBUG ("[%s::config_callback] Setting maximum allowable model radius to: %f.", getName ().c_str (), radius_max);
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting maximum allowable model radius to: %f.",
+      getName().c_str(), radius_max);
     impl_.setRadiusLimits(radius_min, radius_max);
   }
 }
@@ -597,24 +651,21 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback (
   // Enforce that the TF frame and the timestamp are copied
   inliers.header = model.header = fromPCL(cloud->header);
 
-  if (impl_.getModelType () < 0)
-  {
+  if (impl_.getModelType() < 0) {
     NODELET_ERROR("[%s::input_normals_indices_callback] Model type not set!", getName().c_str());
     pub_indices_.publish(boost::make_shared<const PointIndices>(inliers));
     pub_model_.publish(boost::make_shared<const ModelCoefficients>(model));
     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));
     return;
   }
   // If indices are given, check if they are valid
-  if (indices && !isValid (indices))
-  {
+  if (indices && !isValid(indices)) {
     NODELET_ERROR("[%s::input_normals_indices_callback] Invalid indices!", getName().c_str());
     pub_indices_.publish(boost::make_shared<const PointIndices>(inliers));
     pub_model_.publish(boost::make_shared<const ModelCoefficients>(model));
@@ -622,31 +673,46 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback (
   }
 
   /// DEBUG
-  if (indices && !indices->header.frame_id.empty ())
-    NODELET_DEBUG ("[%s::input_normals_indices_callback]\n"
+  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.",
       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 ("input").c_str (),
-                   cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(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_indices_callback]\n"
+      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(),
+      cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
+        *cloud_normals).c_str(), fromPCL(
+        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_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.",
       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 ("input").c_str (),
-                   cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ());
+      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(),
+      cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
+        *cloud_normals).c_str(), fromPCL(
+        cloud_normals->header).stamp.toSec(),
+      cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str());
+  }
   ///
 
 
   // Extra checks for safety
   int cloud_nr_points = cloud->width * cloud->height;
   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)!", getName ().c_str (), cloud_nr_points, cloud_normals_nr_points);
+  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)!",
+      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));
     return;
@@ -656,8 +722,9 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback (
   impl_.setInputNormals(pcl_ptr(cloud_normals));
 
   IndicesPtr indices_ptr;
-  if (indices && !indices->header.frame_id.empty ())
+  if (indices && !indices->header.frame_id.empty()) {
     indices_ptr.reset(new std::vector<int>(indices->indices));
+  }
 
   impl_.setIndices(indices_ptr);
 
@@ -673,8 +740,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 ((int)inliers.indices.size() <= min_inliers_) {
     inliers.indices.clear();
     model.values.clear();
   }
@@ -682,15 +748,16 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback (
   // Publish
   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", 
+  NODELET_DEBUG(
+    "[%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 ())
+  if (inliers.indices.empty()) {
     NODELET_WARN("[%s::input_indices_callback] No inliers found!", getName().c_str());
   }
+}
 
 typedef pcl_ros::SACSegmentation SACSegmentation;
 typedef pcl_ros::SACSegmentationFromNormals SACSegmentationFromNormals;
 PLUGINLIB_EXPORT_CLASS(SACSegmentation, nodelet::Nodelet)
 PLUGINLIB_EXPORT_CLASS(SACSegmentationFromNormals, nodelet::Nodelet)
-

pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp

@@ -48,7 +48,8 @@ pcl_ros::SegmentDifferences::onInit ()
 
   pub_output_ = advertise<PointCloud>(*pnh_, "output", max_queue_size_);
 
-  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
+  NODELET_DEBUG(
+    "[%s::onInit] Nodelet successfully created with the following parameters:\n"
     " - max_queue_size    : %d",
     getName().c_str(),
     max_queue_size_);
@@ -66,20 +67,26 @@ pcl_ros::SegmentDifferences::subscribe ()
 
   // Enable the dynamic reconfigure service
   srv_ = boost::make_shared<dynamic_reconfigure::Server<SegmentDifferencesConfig>>(*pnh_);
-  dynamic_reconfigure::Server<SegmentDifferencesConfig>::CallbackType f =  boost::bind (&SegmentDifferences::config_callback, this, _1, _2);
+  dynamic_reconfigure::Server<SegmentDifferencesConfig>::CallbackType f = boost::bind(
+    &SegmentDifferences::config_callback, this, _1, _2);
   srv_->setCallback(f);
 
-  if (approximate_sync_)
-  {
-    sync_input_target_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloud> > > (max_queue_size_);
+  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_->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_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_->connectInput(sub_input_filter_, sub_target_filter_);
-    sync_input_target_e_->registerCallback (bind (&SegmentDifferences::input_target_callback, this, _1, _2));
+    sync_input_target_e_->registerCallback(
+      bind(
+        &SegmentDifferences::input_target_callback, this,
+        _1, _2));
   }
 }
 
@@ -95,24 +102,26 @@ pcl_ros::SegmentDifferences::unsubscribe ()
 void
 pcl_ros::SegmentDifferences::config_callback(SegmentDifferencesConfig & config, uint32_t level)
 {
-  if (impl_.getDistanceThreshold () != config.distance_threshold)
-  {
+  if (impl_.getDistanceThreshold() != config.distance_threshold) {
     impl_.setDistanceThreshold(config.distance_threshold);
-    NODELET_DEBUG ("[%s::config_callback] Setting new distance threshold to: %f.", getName ().c_str (), config.distance_threshold);
+    NODELET_DEBUG(
+      "[%s::config_callback] Setting new distance threshold to: %f.",
+      getName().c_str(), config.distance_threshold);
   }
 }
 
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
-pcl_ros::SegmentDifferences::input_target_callback (const PointCloudConstPtr &cloud, 
+pcl_ros::SegmentDifferences::input_target_callback(
+  const PointCloudConstPtr & cloud,
   const PointCloudConstPtr & cloud_target)
 {
-  if (pub_output_.getNumSubscribers () <= 0)
+  if (pub_output_.getNumSubscribers() <= 0) {
     return;
+  }
 
-  if (!isValid (cloud) || !isValid (cloud_target, "target")) 
-  {
+  if (!isValid(cloud) || !isValid(cloud_target, "target")) {
     NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str());
     PointCloud output;
     output.header = cloud->header;
@@ -120,12 +129,17 @@ pcl_ros::SegmentDifferences::input_target_callback (const PointCloudConstPtr &cl
     return;
   }
 
-  NODELET_DEBUG ("[%s::input_indices_callback]\n"
+  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.",
     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 ("input").c_str (),
-                 cloud_target->width * cloud_target->height, pcl::getFieldsList (*cloud_target).c_str (), fromPCL(cloud_target->header).stamp.toSec (), cloud_target->header.frame_id.c_str (), pnh_->resolveName ("target").c_str ());
+    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(),
+    cloud_target->width * cloud_target->height, pcl::getFieldsList(*cloud_target).c_str(),
+    fromPCL(cloud_target->header).stamp.toSec(),
+    cloud_target->header.frame_id.c_str(), pnh_->resolveName("target").c_str());
 
   impl_.setInputCloud(pcl_ptr(cloud));
   impl_.setTargetCloud(pcl_ptr(cloud_target));
@@ -134,10 +148,12 @@ pcl_ros::SegmentDifferences::input_target_callback (const PointCloudConstPtr &cl
   impl_.segment(output);
 
   pub_output_.publish(ros_ptr(output.makeShared()));
-  NODELET_DEBUG ("[%s::segmentAndPublish] Published PointCloud2 with %zu points and stamp %f on topic %s", getName ().c_str (),
-                     output.points.size (), fromPCL(output.header).stamp.toSec (), pnh_->resolveName ("output").c_str ());
+  NODELET_DEBUG(
+    "[%s::segmentAndPublish] Published PointCloud2 with %zu points and stamp %f on topic %s",
+    getName().c_str(),
+    output.points.size(), fromPCL(output.header).stamp.toSec(),
+    pnh_->resolveName("output").c_str());
 }
 
 typedef pcl_ros::SegmentDifferences SegmentDifferences;
 PLUGINLIB_EXPORT_CLASS(SegmentDifferences, nodelet::Nodelet)
-

pcl_ros/src/pcl_ros/segmentation/segmentation.cpp

@@ -40,5 +40,3 @@
 //#include "extract_polygonal_prism_data.cpp"
 //#include "sac_segmentation.cpp"
 //#include "segment_differences.cpp"
-
-

pcl_ros/src/pcl_ros/surface/convex_hull.cpp

@@ -52,7 +52,8 @@ pcl_ros::ConvexHull2D::onInit ()
   // ---[ Optional parameters
   pnh_->getParam("use_indices", use_indices_);
 
-  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
+  NODELET_DEBUG(
+    "[%s::onInit] Nodelet successfully created with the following parameters:\n"
     " - use_indices    : %s",
     getName().c_str(),
     (use_indices_) ? "true" : "false");
@@ -65,60 +66,67 @@ void
 pcl_ros::ConvexHull2D::subscribe()
 {
   // If we're supposed to look for PointIndices (indices)
-  if (use_indices_)
-  {
+  if (use_indices_) {
     // Subscribe to the input using a filter
     sub_input_filter_.subscribe(*pnh_, "input", 1);
     // If indices are enabled, subscribe to the indices
     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_);
+    if (approximate_sync_) {
+      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 (bind (&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_a_->registerCallback(
+        bind(
+          &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_);
       // 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 (bind (&ConvexHull2D::input_indices_callback, this, _1, _2));
+      sync_input_indices_e_->registerCallback(
+        bind(
+          &ConvexHull2D::input_indices_callback, this, _1,
+          _2));
     }
-  }
-  else
+  } else {
     // Subscribe in an old fashion to input only (no filters)
-    sub_input_ = pnh_->subscribe<PointCloud> ("input", 1,  bind (&ConvexHull2D::input_indices_callback, this, _1, PointIndicesConstPtr ()));
+    sub_input_ =
+      pnh_->subscribe<PointCloud>(
+      "input", 1,
+      bind(&ConvexHull2D::input_indices_callback, this, _1, PointIndicesConstPtr()));
+  }
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
 pcl_ros::ConvexHull2D::unsubscribe()
 {
-  if (use_indices_)
-  {
+  if (use_indices_) {
     sub_input_filter_.unsubscribe();
     sub_indices_filter_.unsubscribe();
-  }
-  else
+  } else {
     sub_input_.shutdown();
   }
+}
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
-  pcl_ros::ConvexHull2D::input_indices_callback (const PointCloudConstPtr &cloud, 
+pcl_ros::ConvexHull2D::input_indices_callback(
+  const PointCloudConstPtr & cloud,
   const PointIndicesConstPtr & indices)
 {
   // No subscribers, no work
-  if (pub_output_.getNumSubscribers () <= 0 && pub_plane_.getNumSubscribers () <= 0)
+  if (pub_output_.getNumSubscribers() <= 0 && pub_plane_.getNumSubscribers() <= 0) {
     return;
+  }
 
   PointCloud output;
 
   // If cloud is given, check if it's valid
-  if (!isValid (cloud))
-  {
+  if (!isValid(cloud)) {
     NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str());
     // Publish an empty message
     output.header = cloud->header;
@@ -126,8 +134,7 @@ void
     return;
   }
   // If indices are given, check if they are valid
-  if (indices && !isValid (indices, "indices"))
-  {
+  if (indices && !isValid(indices, "indices")) {
     NODELET_ERROR("[%s::input_indices_callback] Invalid indices!", getName().c_str());
     // Publish an empty message
     output.header = cloud->header;
@@ -136,20 +143,30 @@ void
   }
 
   /// DEBUG
-  if (indices)
-    NODELET_DEBUG ("[%s::input_indices_model_callback]\n"
+  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.",
       getName().c_str(),
-                   cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (),
-                   indices->indices.size (), indices->header.stamp.toSec (), 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.", getName ().c_str (), cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ());
+      cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
+        cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(),
+      getMTPrivateNodeHandle().resolveName("input").c_str(),
+      indices->indices.size(), indices->header.stamp.toSec(),
+      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.",
+      getName().c_str(), cloud->width * cloud->height, fromPCL(
+        cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(),
+      getMTPrivateNodeHandle().resolveName("input").c_str());
+  }
 
   // Reset the indices and surface pointers
   IndicesPtr indices_ptr;
-  if (indices)
+  if (indices) {
     indices_ptr.reset(new std::vector<int>(indices->indices));
+  }
 
   impl_.setInputCloud(pcl_ptr(cloud));
   impl_.setIndices(indices_ptr);
@@ -158,8 +175,7 @@ void
   impl_.reconstruct(output);
 
   // If more than 3 points are present, send a PolygonStamped hull too
-  if (output.points.size () >= 3)
-  {
+  if (output.points.size() >= 3) {
     geometry_msgs::PolygonStamped poly;
     poly.header = fromPCL(output.header);
     poly.polygon.points.resize(output.points.size());
@@ -173,19 +189,16 @@ void
     Eigen::Vector4f N = OA.cross3(OB);
     double theta = N.dot(O);
     bool reversed = false;
-    if (theta < (M_PI / 2.0))
+    if (theta < (M_PI / 2.0)) {
       reversed = true;
-    for (size_t i = 0; i < output.points.size (); ++i)
-    {
-      if (reversed)
-      {
+    }
+    for (size_t i = 0; i < output.points.size(); ++i) {
+      if (reversed) {
         size_t j = output.points.size() - i - 1;
         poly.polygon.points[i].x = output.points[j].x;
         poly.polygon.points[i].y = output.points[j].y;
         poly.polygon.points[i].z = output.points[j].z;
-      }
-      else
-      {
+      } else {
         poly.polygon.points[i].x = output.points[i].x;
         poly.polygon.points[i].y = output.points[i].y;
         poly.polygon.points[i].z = output.points[i].z;
@@ -200,4 +213,3 @@ void
 
 typedef pcl_ros::ConvexHull2D ConvexHull2D;
 PLUGINLIB_EXPORT_CLASS(ConvexHull2D, nodelet::Nodelet)
-

pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp

@@ -49,27 +49,31 @@ pcl_ros::MovingLeastSquares::onInit ()
   pub_normals_ = advertise<NormalCloudOut>(*pnh_, "normals", max_queue_size_);
 
   //if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("search_radius", search_radius_))
-  if (!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] Need a 'search_radius' parameter to be set before continuing!", getName ().c_str ());
+    NODELET_ERROR(
+      "[%s::onInit] Need a 'search_radius' parameter to be set before continuing!",
+      getName().c_str());
     return;
   }
-  if (!pnh_->getParam ("spatial_locator", spatial_locator_type_))
-  { 
-    NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
+  if (!pnh_->getParam("spatial_locator", spatial_locator_type_)) {
+    NODELET_ERROR(
+      "[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!",
+      getName().c_str());
     return;
   }
 
   // Enable the dynamic reconfigure service
   srv_ = boost::make_shared<dynamic_reconfigure::Server<MLSConfig>>(*pnh_);
-  dynamic_reconfigure::Server<MLSConfig>::CallbackType f = boost::bind (&MovingLeastSquares::config_callback, this, _1, _2 );
+  dynamic_reconfigure::Server<MLSConfig>::CallbackType f = boost::bind(
+    &MovingLeastSquares::config_callback, this, _1, _2);
   srv_->setCallback(f);
 
   // ---[ Optional parameters
   pnh_->getParam("use_indices", use_indices_);
 
-  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
+  NODELET_DEBUG(
+    "[%s::onInit] Nodelet successfully created with the following parameters:\n"
     " - use_indices    : %s",
     getName().c_str(),
     (use_indices_) ? "true" : "false");
@@ -82,54 +86,62 @@ void
 pcl_ros::MovingLeastSquares::subscribe()
 {
   // If we're supposed to look for PointIndices (indices)
-  if (use_indices_)
-  {
+  if (use_indices_) {
     // Subscribe to the input using a filter
     sub_input_filter_.subscribe(*pnh_, "input", 1);
     // If indices are enabled, subscribe to the indices
     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_);
+    if (approximate_sync_) {
+      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 (bind (&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_a_->registerCallback(
+        bind(
+          &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_);
       // 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 (bind (&MovingLeastSquares::input_indices_callback, this, _1, _2));
+      sync_input_indices_e_->registerCallback(
+        bind(
+          &MovingLeastSquares::input_indices_callback,
+          this, _1, _2));
     }
-  }
-  else
+  } else {
     // Subscribe in an old fashion to input only (no filters)
-    sub_input_ = pnh_->subscribe<PointCloudIn> ("input", 1,  bind (&MovingLeastSquares::input_indices_callback, this, _1, PointIndicesConstPtr ()));
+    sub_input_ =
+      pnh_->subscribe<PointCloudIn>(
+      "input", 1,
+      bind(&MovingLeastSquares::input_indices_callback, this, _1, PointIndicesConstPtr()));
+  }
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
 pcl_ros::MovingLeastSquares::unsubscribe()
 {
-  if (use_indices_)
-  {
+  if (use_indices_) {
     sub_input_filter_.unsubscribe();
     sub_indices_filter_.unsubscribe();
-  }
-  else
+  } else {
     sub_input_.shutdown();
   }
+}
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
-pcl_ros::MovingLeastSquares::input_indices_callback (const PointCloudInConstPtr &cloud, 
+pcl_ros::MovingLeastSquares::input_indices_callback(
+  const PointCloudInConstPtr & cloud,
   const PointIndicesConstPtr & indices)
 {
   // No subscribers, no work
-  if (pub_output_.getNumSubscribers () <= 0 && pub_normals_.getNumSubscribers () <= 0)
+  if (pub_output_.getNumSubscribers() <= 0 && pub_normals_.getNumSubscribers() <= 0) {
     return;
+  }
 
   // Output points have the same type as the input, they are only smoothed
   PointCloudIn output;
@@ -138,16 +150,14 @@ pcl_ros::MovingLeastSquares::input_indices_callback (const PointCloudInConstPtr
   NormalCloudOut::Ptr normals(new NormalCloudOut());
 
   // If cloud is given, check if it's valid
-  if (!isValid (cloud)) 
-  {
+  if (!isValid(cloud)) {
     NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str());
     output.header = cloud->header;
     pub_output_.publish(ros_ptr(output.makeShared()));
     return;
   }
   // If indices are given, check if they are valid
-  if (indices && !isValid (indices, "indices"))
-  {
+  if (indices && !isValid(indices, "indices")) {
     NODELET_ERROR("[%s::input_indices_callback] Invalid indices!", getName().c_str());
     output.header = cloud->header;
     pub_output_.publish(ros_ptr(output.makeShared()));
@@ -155,23 +165,33 @@ pcl_ros::MovingLeastSquares::input_indices_callback (const PointCloudInConstPtr
   }
 
   /// DEBUG
-  if (indices)
-    NODELET_DEBUG ("[%s::input_indices_model_callback]\n"
+  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.",
       getName().c_str(),
-                   cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (),
-                   indices->indices.size (), indices->header.stamp.toSec (), 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.", getName ().c_str (), cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ());
+      cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
+        cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(),
+      getMTPrivateNodeHandle().resolveName("input").c_str(),
+      indices->indices.size(), indices->header.stamp.toSec(),
+      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.",
+      getName().c_str(), cloud->width * cloud->height, fromPCL(
+        cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(),
+      getMTPrivateNodeHandle().resolveName("input").c_str());
+  }
   ///
 
   // Reset the indices and surface pointers
   impl_.setInputCloud(pcl_ptr(cloud));
 
   IndicesPtr indices_ptr;
-  if (indices)
+  if (indices) {
     indices_ptr.reset(new std::vector<int>(indices->indices));
+  }
 
   impl_.setIndices(indices_ptr);
 
@@ -198,31 +218,30 @@ pcl_ros::MovingLeastSquares::config_callback (MLSConfig &config, uint32_t level)
     k_ = config.k_search;
     NODELET_DEBUG ("[config_callback] Setting the k_search to: %d.", k_);
   }*/
-  if (search_radius_ != config.search_radius)
-  {
+  if (search_radius_ != config.search_radius) {
     search_radius_ = config.search_radius;
     NODELET_DEBUG("[config_callback] Setting the search radius: %f.", search_radius_);
     impl_.setSearchRadius(search_radius_);
   }
-  if (spatial_locator_type_ != config.spatial_locator)
-  {
+  if (spatial_locator_type_ != config.spatial_locator) {
     spatial_locator_type_ = config.spatial_locator;
-    NODELET_DEBUG ("[config_callback] Setting the spatial locator to type: %d.", spatial_locator_type_);
+    NODELET_DEBUG(
+      "[config_callback] Setting the spatial locator to type: %d.",
+      spatial_locator_type_);
   }
-  if (use_polynomial_fit_ != config.use_polynomial_fit)
-  {
+  if (use_polynomial_fit_ != config.use_polynomial_fit) {
     use_polynomial_fit_ = config.use_polynomial_fit;
-    NODELET_DEBUG ("[config_callback] Setting the use_polynomial_fit flag to: %d.", use_polynomial_fit_);
+    NODELET_DEBUG(
+      "[config_callback] Setting the use_polynomial_fit flag to: %d.",
+      use_polynomial_fit_);
     impl_.setPolynomialFit(use_polynomial_fit_);
   }
-  if (polynomial_order_ != config.polynomial_order)
-  {
+  if (polynomial_order_ != config.polynomial_order) {
     polynomial_order_ = config.polynomial_order;
     NODELET_DEBUG("[config_callback] Setting the polynomial order to: %d.", polynomial_order_);
     impl_.setPolynomialOrder(polynomial_order_);
   }
-  if (gaussian_parameter_ != config.gaussian_parameter)
-  {
+  if (gaussian_parameter_ != config.gaussian_parameter) {
     gaussian_parameter_ = config.gaussian_parameter;
     NODELET_DEBUG("[config_callback] Setting the gaussian parameter to: %f.", gaussian_parameter_);
     impl_.setSqrGaussParam(gaussian_parameter_ * gaussian_parameter_);

pcl_ros/src/pcl_ros/surface/surface.cpp

@@ -44,5 +44,3 @@
 // Include the implementations here instead of compiling them separately to speed up compile time
 //#include "convex_hull.cpp"
 //#include "moving_least_squares.cpp"
-
-

pcl_ros/src/test/test_tf_message_filter_pcl.cpp

@@ -79,9 +79,9 @@ class Notification
 {
 public:
   Notification(int expected_count)
-  : count_(0)
-  , expected_count_(expected_count)
-  , failure_count_(0)
+  : count_(0),
+    expected_count_(expected_count),
+    failure_count_(0)
   {
   }
 
@@ -144,7 +144,11 @@ TEST(MessageFilter, preexistingTransforms)
   ros::Time stamp = ros::Time::now();
   setStamp(stamp, msg->header.stamp);
 
-  tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2");
+  tf::StampedTransform transform(tf::Transform(
+      tf::Quaternion(0, 0, 0, 1), tf::Vector3(
+        1, 2,
+        3)), stamp, "frame1",
+    "frame2");
   tf_client.setTransform(transform);
 
   msg->header.frame_id = "frame2";
@@ -169,7 +173,11 @@ TEST(MessageFilter, postTransforms)
 
   EXPECT_EQ(0, n.count_);
 
-  tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2");
+  tf::StampedTransform transform(tf::Transform(
+      tf::Quaternion(0, 0, 0, 1), tf::Vector3(
+        1, 2,
+        3)), stamp, "frame1",
+    "frame2");
   tf_client.setTransform(transform);
 
   ros::WallDuration(0.1).sleep();
@@ -190,8 +198,7 @@ TEST(MessageFilter, queueSize)
   std::uint64_t pcl_stamp;
   setStamp(stamp, pcl_stamp);
 
-  for (int i = 0; i < 20; ++i)
-  {
+  for (int i = 0; i < 20; ++i) {
     PCDType::Ptr msg(new PCDType);
     msg->header.stamp = pcl_stamp;
     msg->header.frame_id = "frame2";
@@ -202,7 +209,11 @@ TEST(MessageFilter, queueSize)
   EXPECT_EQ(0, n.count_);
   EXPECT_EQ(10, n.failure_count_);
 
-  tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2");
+  tf::StampedTransform transform(tf::Transform(
+      tf::Quaternion(0, 0, 0, 1), tf::Vector3(
+        1, 2,
+        3)), stamp, "frame1",
+    "frame2");
   tf_client.setTransform(transform);
 
   ros::WallDuration(0.1).sleep();
@@ -224,7 +235,11 @@ TEST(MessageFilter, setTargetFrame)
   setStamp(stamp, msg->header.stamp);
   msg->header.frame_id = "frame2";
 
-  tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1000", "frame2");
+  tf::StampedTransform transform(tf::Transform(
+      tf::Quaternion(0, 0, 0, 1), tf::Vector3(
+        1, 2,
+        3)), stamp, "frame1000",
+    "frame2");
   tf_client.setTransform(transform);
 
   filter.add(msg);
@@ -249,7 +264,11 @@ TEST(MessageFilter, multipleTargetFrames)
   PCDType::Ptr msg(new PCDType);
   setStamp(stamp, msg->header.stamp);
 
-  tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame3");
+  tf::StampedTransform transform(tf::Transform(
+      tf::Quaternion(0, 0, 0, 1), tf::Vector3(
+        1, 2,
+        3)), stamp, "frame1",
+    "frame3");
   tf_client.setTransform(transform);
 
   msg->header.frame_id = "frame3";
@@ -283,7 +302,11 @@ TEST(MessageFilter, tolerance)
   ros::Time stamp = ros::Time::now();
   std::uint64_t pcl_stamp;
   setStamp(stamp, pcl_stamp);
-  tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2");
+  tf::StampedTransform transform(tf::Transform(
+      tf::Quaternion(0, 0, 0, 1), tf::Vector3(
+        1, 2,
+        3)), stamp, "frame1",
+    "frame2");
   tf_client.setTransform(transform);
 
   PCDType::Ptr msg(new PCDType);
@@ -323,7 +346,11 @@ TEST(MessageFilter, outTheBackFailure)
   PCDType::Ptr msg(new PCDType);
   setStamp(stamp, msg->header.stamp);
 
-  tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2");
+  tf::StampedTransform transform(tf::Transform(
+      tf::Quaternion(0, 0, 0, 1), tf::Vector3(
+        1, 2,
+        3)), stamp, "frame1",
+    "frame2");
   tf_client.setTransform(transform);
 
   transform.stamp_ = stamp + ros::Duration(10000);
@@ -366,7 +393,8 @@ TEST(MessageFilter, removeCallback)
     tf_listener.reset(new tf::TransformListener());
     // Have callback fire at high rate to increase chances of race condition
     tf_filter.reset(
-      new tf::MessageFilter<PCDType>(*tf_listener,
+      new tf::MessageFilter<PCDType>(
+        *tf_listener,
         "map", 5, threaded_nh,
         ros::Duration(0.000001)));
 

pcl_ros/src/transforms.cpp

@@ -45,30 +45,25 @@ namespace pcl_ros
 {
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 bool
-transformPointCloud (const std::string &target_frame, const sensor_msgs::PointCloud2 &in, 
+transformPointCloud(
+  const std::string & target_frame, const sensor_msgs::PointCloud2 & in,
   sensor_msgs::PointCloud2 & out, const tf::TransformListener & tf_listener)
 {
-  if (in.header.frame_id == target_frame)
-  {
+  if (in.header.frame_id == target_frame) {
     out = in;
-    return (true);
+    return true;
   }
 
   // Get the TF transform
   tf::StampedTransform transform;
-  try
-  {
+  try {
     tf_listener.lookupTransform(target_frame, in.header.frame_id, in.header.stamp, transform);
-  }
-  catch (tf::LookupException &e)
-  {
+  } catch (tf::LookupException & e) {
     ROS_ERROR("%s", e.what());
-    return (false);
-  }
-  catch (tf::ExtrapolationException &e)
-  {
+    return false;
+  } catch (tf::ExtrapolationException & e) {
     ROS_ERROR("%s", e.what());
-    return (false);
+    return false;
   }
 
   // Convert the TF transform to Eigen format
@@ -78,16 +73,16 @@ transformPointCloud (const std::string &target_frame, const sensor_msgs::PointCl
   transformPointCloud(eigen_transform, in, out);
 
   out.header.frame_id = target_frame;
-  return (true);
+  return true;
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 void
-transformPointCloud (const std::string &target_frame, const tf::Transform &net_transform,
+transformPointCloud(
+  const std::string & target_frame, const tf::Transform & net_transform,
   const sensor_msgs::PointCloud2 & in, sensor_msgs::PointCloud2 & out)
 {
-  if (in.header.frame_id == target_frame)
-  {
+  if (in.header.frame_id == target_frame) {
     out = in;
     return;
   }
@@ -103,7 +98,8 @@ transformPointCloud (const std::string &target_frame, const tf::Transform &net_t
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 void
-transformPointCloud (const Eigen::Matrix4f &transform, const sensor_msgs::PointCloud2 &in,
+transformPointCloud(
+  const Eigen::Matrix4f & transform, const sensor_msgs::PointCloud2 & in,
   sensor_msgs::PointCloud2 & out)
 {
   // Get X-Y-Z indices
@@ -111,8 +107,7 @@ transformPointCloud (const Eigen::Matrix4f &transform, const sensor_msgs::PointC
   int y_idx = pcl::getFieldIndex(in, "y");
   int z_idx = pcl::getFieldIndex(in, "z");
 
-  if (x_idx == -1 || y_idx == -1 || z_idx == -1)
-  {
+  if (x_idx == -1 || y_idx == -1 || z_idx == -1) {
     ROS_ERROR("Input dataset has no X-Y-Z coordinates! Cannot convert to Eigen format.");
     return;
   }
@@ -129,8 +124,7 @@ transformPointCloud (const Eigen::Matrix4f &transform, const sensor_msgs::PointC
   int dist_idx = pcl::getFieldIndex(in, "distance");
 
   // Copy the other data
-  if (&in != &out)
-  {
+  if (&in != &out) {
     out.header = in.header;
     out.height = in.height;
     out.width = in.width;
@@ -144,37 +138,31 @@ transformPointCloud (const Eigen::Matrix4f &transform, const sensor_msgs::PointC
     memcpy(&out.data[0], &in.data[0], in.data.size());
   }
 
-  Eigen::Array4i xyz_offset (in.fields[x_idx].offset, in.fields[y_idx].offset, in.fields[z_idx].offset, 0);
+  Eigen::Array4i xyz_offset(in.fields[x_idx].offset, in.fields[y_idx].offset,
+    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);
+  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_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]));
-    if (!std::isfinite (pt[0]) || !std::isfinite (pt[1]) || !std::isfinite (pt[2]))
-    {
-      if (distance_ptr==NULL || !std::isfinite(*distance_ptr))  // Invalid point
-      {
+    if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2])) {
+      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";
       }
-    }
-    else
-    {
+    } else {
       pt_out = transform * pt;
     }
 
-    if (max_range_point)
-    {
+    if (max_range_point) {
       // Save x value in distance again
       *(float *)(&out.data[distance_ptr_offset]) = pt_out[0];
       pt_out[0] = std::numeric_limits<float>::quiet_NaN();
@@ -190,11 +178,9 @@ transformPointCloud (const Eigen::Matrix4f &transform, const sensor_msgs::PointC
 
   // Check if the viewpoint information is present
   int vp_idx = pcl::getFieldIndex(in, "vp_x");
-  if (vp_idx != -1)
-  {
+  if (vp_idx != -1) {
     // Transform the viewpoint info too
-    for (size_t i = 0; i < out.width * out.height; ++i)
-    {
+    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];
       // Assume vp_x, vp_y, vp_z are consecutive
       Eigen::Vector4f vp_in(pstep[0], pstep[1], pstep[2], 1);
@@ -229,53 +215,173 @@ transformAsMatrix (const tf::Transform& bt, Eigen::Matrix4f &out_mat)
 } // namespace pcl_ros
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-template void pcl_ros::transformPointCloudWithNormals<pcl::PointNormal> (const pcl::PointCloud <pcl::PointNormal> &, pcl::PointCloud <pcl::PointNormal> &, const tf::Transform &);
-template void pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal> (const pcl::PointCloud <pcl::PointXYZRGBNormal> &, pcl::PointCloud <pcl::PointXYZRGBNormal> &, const tf::Transform &);
-template void pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal> (const pcl::PointCloud <pcl::PointXYZINormal> &, pcl::PointCloud <pcl::PointXYZINormal> &, const tf::Transform &);
+template void pcl_ros::transformPointCloudWithNormals<pcl::PointNormal>(
+  const pcl::PointCloud<pcl::PointNormal> &, pcl::PointCloud<pcl::PointNormal> &,
+  const tf::Transform &);
+template void pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal>(
+  const pcl::PointCloud<pcl::PointXYZRGBNormal> &, pcl::PointCloud<pcl::PointXYZRGBNormal> &,
+  const tf::Transform &);
+template void pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal>(
+  const pcl::PointCloud<pcl::PointXYZINormal> &, pcl::PointCloud<pcl::PointXYZINormal> &,
+  const tf::Transform &);
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-template bool pcl_ros::transformPointCloudWithNormals<pcl::PointNormal> (const std::string &, const pcl::PointCloud<pcl::PointNormal> &, pcl::PointCloud<pcl::PointNormal> &, const tf::TransformListener &);
-template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal> (const std::string &, const pcl::PointCloud<pcl::PointXYZRGBNormal> &, pcl::PointCloud<pcl::PointXYZRGBNormal> &, const tf::TransformListener &);
-template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal> (const std::string &, const pcl::PointCloud<pcl::PointXYZINormal> &, pcl::PointCloud<pcl::PointXYZINormal> &, const tf::TransformListener &);
+template bool pcl_ros::transformPointCloudWithNormals<pcl::PointNormal>(
+  const std::string &,
+  const pcl::PointCloud<pcl::PointNormal> &, pcl::PointCloud<pcl::PointNormal> &,
+  const tf::TransformListener &);
+template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal>(
+  const std::string &,
+  const pcl::PointCloud<pcl::PointXYZRGBNormal> &, pcl::PointCloud<pcl::PointXYZRGBNormal> &,
+  const tf::TransformListener &);
+template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal>(
+  const std::string &,
+  const pcl::PointCloud<pcl::PointXYZINormal> &, pcl::PointCloud<pcl::PointXYZINormal> &,
+  const tf::TransformListener &);
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-template bool pcl_ros::transformPointCloudWithNormals<pcl::PointNormal> (const std::string &, const ros::Time &, const pcl::PointCloud<pcl::PointNormal> &, const std::string &, pcl::PointCloud <pcl::PointNormal> &, const tf::TransformListener &);
-template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal> (const std::string &, const ros::Time &, const pcl::PointCloud<pcl::PointXYZRGBNormal> &, const std::string &, pcl::PointCloud <pcl::PointXYZRGBNormal> &, const tf::TransformListener &);
-template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal> (const std::string &, const ros::Time &, const pcl::PointCloud<pcl::PointXYZINormal> &, const std::string &, pcl::PointCloud <pcl::PointXYZINormal> &, const tf::TransformListener &);
+template bool pcl_ros::transformPointCloudWithNormals<pcl::PointNormal>(
+  const std::string &,
+  const ros::Time &,
+  const pcl::PointCloud<pcl::PointNormal> &, const std::string &,
+  pcl::PointCloud<pcl::PointNormal> &, const tf::TransformListener &);
+template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal>(
+  const std::string &,
+  const ros::Time &,
+  const pcl::PointCloud<pcl::PointXYZRGBNormal> &, const std::string &,
+  pcl::PointCloud<pcl::PointXYZRGBNormal> &, const tf::TransformListener &);
+template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal>(
+  const std::string &,
+  const ros::Time &,
+  const pcl::PointCloud<pcl::PointXYZINormal> &, const std::string &,
+  pcl::PointCloud<pcl::PointXYZINormal> &, const tf::TransformListener &);
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-template void pcl_ros::transformPointCloud<pcl::PointXYZ> (const pcl::PointCloud <pcl::PointXYZ> &, pcl::PointCloud <pcl::PointXYZ> &, const tf::Transform &);
-template void pcl_ros::transformPointCloud<pcl::PointXYZI> (const pcl::PointCloud <pcl::PointXYZI> &, pcl::PointCloud <pcl::PointXYZI> &, const tf::Transform &);
-template void pcl_ros::transformPointCloud<pcl::PointXYZRGBA> (const pcl::PointCloud <pcl::PointXYZRGBA> &, pcl::PointCloud <pcl::PointXYZRGBA> &, const tf::Transform &);
-template void pcl_ros::transformPointCloud<pcl::PointXYZRGB> (const pcl::PointCloud <pcl::PointXYZRGB> &, pcl::PointCloud <pcl::PointXYZRGB> &, const tf::Transform &);
-template void pcl_ros::transformPointCloud<pcl::InterestPoint> (const pcl::PointCloud <pcl::InterestPoint> &, pcl::PointCloud <pcl::InterestPoint> &, const tf::Transform &);
-template void pcl_ros::transformPointCloud<pcl::PointNormal> (const pcl::PointCloud <pcl::PointNormal> &, pcl::PointCloud <pcl::PointNormal> &, const tf::Transform &);
-template void pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal> (const pcl::PointCloud <pcl::PointXYZRGBNormal> &, pcl::PointCloud <pcl::PointXYZRGBNormal> &, const tf::Transform &);
-template void pcl_ros::transformPointCloud<pcl::PointXYZINormal> (const pcl::PointCloud <pcl::PointXYZINormal> &, pcl::PointCloud <pcl::PointXYZINormal> &, const tf::Transform &);
-template void pcl_ros::transformPointCloud<pcl::PointWithRange> (const pcl::PointCloud <pcl::PointWithRange> &, pcl::PointCloud <pcl::PointWithRange> &, const tf::Transform &);
-template void pcl_ros::transformPointCloud<pcl::PointWithViewpoint> (const pcl::PointCloud <pcl::PointWithViewpoint> &, pcl::PointCloud <pcl::PointWithViewpoint> &, const tf::Transform &);
+template void pcl_ros::transformPointCloud<pcl::PointXYZ>(
+  const pcl::PointCloud<pcl::PointXYZ> &,
+  pcl::PointCloud<pcl::PointXYZ> &,
+  const tf::Transform &);
+template void pcl_ros::transformPointCloud<pcl::PointXYZI>(
+  const pcl::PointCloud<pcl::PointXYZI> &,
+  pcl::PointCloud<pcl::PointXYZI> &,
+  const tf::Transform &);
+template void pcl_ros::transformPointCloud<pcl::PointXYZRGBA>(
+  const pcl::PointCloud<pcl::PointXYZRGBA> &, pcl::PointCloud<pcl::PointXYZRGBA> &,
+  const tf::Transform &);
+template void pcl_ros::transformPointCloud<pcl::PointXYZRGB>(
+  const pcl::PointCloud<pcl::PointXYZRGB> &, pcl::PointCloud<pcl::PointXYZRGB> &,
+  const tf::Transform &);
+template void pcl_ros::transformPointCloud<pcl::InterestPoint>(
+  const pcl::PointCloud<pcl::InterestPoint> &, pcl::PointCloud<pcl::InterestPoint> &,
+  const tf::Transform &);
+template void pcl_ros::transformPointCloud<pcl::PointNormal>(
+  const pcl::PointCloud<pcl::PointNormal> &, pcl::PointCloud<pcl::PointNormal> &,
+  const tf::Transform &);
+template void pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal>(
+  const pcl::PointCloud<pcl::PointXYZRGBNormal> &, pcl::PointCloud<pcl::PointXYZRGBNormal> &,
+  const tf::Transform &);
+template void pcl_ros::transformPointCloud<pcl::PointXYZINormal>(
+  const pcl::PointCloud<pcl::PointXYZINormal> &, pcl::PointCloud<pcl::PointXYZINormal> &,
+  const tf::Transform &);
+template void pcl_ros::transformPointCloud<pcl::PointWithRange>(
+  const pcl::PointCloud<pcl::PointWithRange> &, pcl::PointCloud<pcl::PointWithRange> &,
+  const tf::Transform &);
+template void pcl_ros::transformPointCloud<pcl::PointWithViewpoint>(
+  const pcl::PointCloud<pcl::PointWithViewpoint> &, pcl::PointCloud<pcl::PointWithViewpoint> &,
+  const tf::Transform &);
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-template bool pcl_ros::transformPointCloud<pcl::PointXYZ> (const std::string &, const pcl::PointCloud <pcl::PointXYZ> &, pcl::PointCloud <pcl::PointXYZ> &, const tf::TransformListener &);
-template bool pcl_ros::transformPointCloud<pcl::PointXYZI> (const std::string &, const pcl::PointCloud <pcl::PointXYZI> &, pcl::PointCloud <pcl::PointXYZI> &, const tf::TransformListener &);
-template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBA> (const std::string &, const pcl::PointCloud <pcl::PointXYZRGBA> &, pcl::PointCloud <pcl::PointXYZRGBA> &, const tf::TransformListener &);
-template bool pcl_ros::transformPointCloud<pcl::PointXYZRGB> (const std::string &, const pcl::PointCloud <pcl::PointXYZRGB> &, pcl::PointCloud <pcl::PointXYZRGB> &, const tf::TransformListener &);
-template bool pcl_ros::transformPointCloud<pcl::InterestPoint> (const std::string &, const pcl::PointCloud <pcl::InterestPoint> &, pcl::PointCloud <pcl::InterestPoint> &, const tf::TransformListener &);
-template bool pcl_ros::transformPointCloud<pcl::PointNormal> (const std::string &, const pcl::PointCloud <pcl::PointNormal> &, pcl::PointCloud <pcl::PointNormal> &, const tf::TransformListener &);
-template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal> (const std::string &, const pcl::PointCloud <pcl::PointXYZRGBNormal> &, pcl::PointCloud <pcl::PointXYZRGBNormal> &, const tf::TransformListener &);
-template bool pcl_ros::transformPointCloud<pcl::PointXYZINormal> (const std::string &, const pcl::PointCloud <pcl::PointXYZINormal> &, pcl::PointCloud <pcl::PointXYZINormal> &, const tf::TransformListener &);
-template bool pcl_ros::transformPointCloud<pcl::PointWithRange> (const std::string &, const pcl::PointCloud <pcl::PointWithRange> &, pcl::PointCloud <pcl::PointWithRange> &, const tf::TransformListener &);
-template bool pcl_ros::transformPointCloud<pcl::PointWithViewpoint> (const std::string &, const pcl::PointCloud <pcl::PointWithViewpoint> &, pcl::PointCloud <pcl::PointWithViewpoint> &, const tf::TransformListener &);
+template bool pcl_ros::transformPointCloud<pcl::PointXYZ>(
+  const std::string &,
+  const pcl::PointCloud<pcl::PointXYZ> &,
+  pcl::PointCloud<pcl::PointXYZ> &,
+  const tf::TransformListener &);
+template bool pcl_ros::transformPointCloud<pcl::PointXYZI>(
+  const std::string &,
+  const pcl::PointCloud<pcl::PointXYZI> &,
+  pcl::PointCloud<pcl::PointXYZI> &,
+  const tf::TransformListener &);
+template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBA>(
+  const std::string &,
+  const pcl::PointCloud<pcl::PointXYZRGBA> &, pcl::PointCloud<pcl::PointXYZRGBA> &,
+  const tf::TransformListener &);
+template bool pcl_ros::transformPointCloud<pcl::PointXYZRGB>(
+  const std::string &,
+  const pcl::PointCloud<pcl::PointXYZRGB> &, pcl::PointCloud<pcl::PointXYZRGB> &,
+  const tf::TransformListener &);
+template bool pcl_ros::transformPointCloud<pcl::InterestPoint>(
+  const std::string &,
+  const pcl::PointCloud<pcl::InterestPoint> &, pcl::PointCloud<pcl::InterestPoint> &,
+  const tf::TransformListener &);
+template bool pcl_ros::transformPointCloud<pcl::PointNormal>(
+  const std::string &,
+  const pcl::PointCloud<pcl::PointNormal> &, pcl::PointCloud<pcl::PointNormal> &,
+  const tf::TransformListener &);
+template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal>(
+  const std::string &,
+  const pcl::PointCloud<pcl::PointXYZRGBNormal> &, pcl::PointCloud<pcl::PointXYZRGBNormal> &,
+  const tf::TransformListener &);
+template bool pcl_ros::transformPointCloud<pcl::PointXYZINormal>(
+  const std::string &,
+  const pcl::PointCloud<pcl::PointXYZINormal> &, pcl::PointCloud<pcl::PointXYZINormal> &,
+  const tf::TransformListener &);
+template bool pcl_ros::transformPointCloud<pcl::PointWithRange>(
+  const std::string &,
+  const pcl::PointCloud<pcl::PointWithRange> &, pcl::PointCloud<pcl::PointWithRange> &,
+  const tf::TransformListener &);
+template bool pcl_ros::transformPointCloud<pcl::PointWithViewpoint>(
+  const std::string &,
+  const pcl::PointCloud<pcl::PointWithViewpoint> &, pcl::PointCloud<pcl::PointWithViewpoint> &,
+  const tf::TransformListener &);
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-template bool pcl_ros::transformPointCloud<pcl::PointXYZ> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointXYZ> &, const std::string &, pcl::PointCloud <pcl::PointXYZ> &, const tf::TransformListener &);
-template bool pcl_ros::transformPointCloud<pcl::PointXYZI> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointXYZI> &, const std::string &, pcl::PointCloud <pcl::PointXYZI> &, const tf::TransformListener &);
-template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBA> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointXYZRGBA> &, const std::string &, pcl::PointCloud <pcl::PointXYZRGBA> &, const tf::TransformListener &);
-template bool pcl_ros::transformPointCloud<pcl::PointXYZRGB> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointXYZRGB> &, const std::string &, pcl::PointCloud <pcl::PointXYZRGB> &, const tf::TransformListener &);
-template bool pcl_ros::transformPointCloud<pcl::InterestPoint> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::InterestPoint> &, const std::string &, pcl::PointCloud <pcl::InterestPoint> &, const tf::TransformListener &);
-template bool pcl_ros::transformPointCloud<pcl::PointNormal> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointNormal> &, const std::string &, pcl::PointCloud <pcl::PointNormal> &, const tf::TransformListener &);
-template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointXYZRGBNormal> &, const std::string &, pcl::PointCloud <pcl::PointXYZRGBNormal> &, const tf::TransformListener &);
-template bool pcl_ros::transformPointCloud<pcl::PointXYZINormal> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointXYZINormal> &, const std::string &, pcl::PointCloud <pcl::PointXYZINormal> &, const tf::TransformListener &);
-template bool pcl_ros::transformPointCloud<pcl::PointWithRange> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointWithRange> &, const std::string &, pcl::PointCloud <pcl::PointWithRange> &, const tf::TransformListener &);
-template bool pcl_ros::transformPointCloud<pcl::PointWithViewpoint> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointWithViewpoint> &, const std::string &, pcl::PointCloud <pcl::PointWithViewpoint> &, const tf::TransformListener &);
-
+template bool pcl_ros::transformPointCloud<pcl::PointXYZ>(
+  const std::string &, const ros::Time &,
+  const pcl::PointCloud<pcl::PointXYZ> &,
+  const std::string &,
+  pcl::PointCloud<pcl::PointXYZ> &,
+  const tf::TransformListener &);
+template bool pcl_ros::transformPointCloud<pcl::PointXYZI>(
+  const std::string &, const ros::Time &,
+  const pcl::PointCloud<pcl::PointXYZI> &,
+  const std::string &,
+  pcl::PointCloud<pcl::PointXYZI> &,
+  const tf::TransformListener &);
+template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBA>(
+  const std::string &,
+  const ros::Time &,
+  const pcl::PointCloud<pcl::PointXYZRGBA> &, const std::string &,
+  pcl::PointCloud<pcl::PointXYZRGBA> &, const tf::TransformListener &);
+template bool pcl_ros::transformPointCloud<pcl::PointXYZRGB>(
+  const std::string &, const ros::Time &,
+  const pcl::PointCloud<pcl::PointXYZRGB> &, const std::string &,
+  pcl::PointCloud<pcl::PointXYZRGB> &, const tf::TransformListener &);
+template bool pcl_ros::transformPointCloud<pcl::InterestPoint>(
+  const std::string &,
+  const ros::Time &,
+  const pcl::PointCloud<pcl::InterestPoint> &, const std::string &,
+  pcl::PointCloud<pcl::InterestPoint> &, const tf::TransformListener &);
+template bool pcl_ros::transformPointCloud<pcl::PointNormal>(
+  const std::string &, const ros::Time &,
+  const pcl::PointCloud<pcl::PointNormal> &, const std::string &,
+  pcl::PointCloud<pcl::PointNormal> &, const tf::TransformListener &);
+template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal>(
+  const std::string &,
+  const ros::Time &,
+  const pcl::PointCloud<pcl::PointXYZRGBNormal> &, const std::string &,
+  pcl::PointCloud<pcl::PointXYZRGBNormal> &, const tf::TransformListener &);
+template bool pcl_ros::transformPointCloud<pcl::PointXYZINormal>(
+  const std::string &,
+  const ros::Time &,
+  const pcl::PointCloud<pcl::PointXYZINormal> &, const std::string &,
+  pcl::PointCloud<pcl::PointXYZINormal> &, const tf::TransformListener &);
+template bool pcl_ros::transformPointCloud<pcl::PointWithRange>(
+  const std::string &,
+  const ros::Time &,
+  const pcl::PointCloud<pcl::PointWithRange> &, const std::string &,
+  pcl::PointCloud<pcl::PointWithRange> &, const tf::TransformListener &);
+template bool pcl_ros::transformPointCloud<pcl::PointWithViewpoint>(
+  const std::string &,
+  const ros::Time &,
+  const pcl::PointCloud<pcl::PointWithViewpoint> &, const std::string &,
+  pcl::PointCloud<pcl::PointWithViewpoint> &, const tf::TransformListener &);

pcl_ros/tools/bag_to_pcd.cpp

@@ -64,11 +64,12 @@ int
 main(int argc, char ** argv)
 {
   ros::init(argc, argv, "bag_to_pcd");
-  if (argc < 4)
-  {
-    std::cerr << "Syntax is: " << argv[0] << " <file_in.bag> <topic> <output_directory> [<target_frame>]" << std::endl;
-    std::cerr << "Example: " << argv[0] << " data.bag /laser_tilt_cloud ./pointclouds /base_link" << std::endl;
-    return (-1);
+  if (argc < 4) {
+    std::cerr << "Syntax is: " << argv[0] <<
+      " <file_in.bag> <topic> <output_directory> [<target_frame>]" << std::endl;
+    std::cerr << "Example: " << argv[0] << " data.bag /laser_tilt_cloud ./pointclouds /base_link" <<
+      std::endl;
+    return -1;
   }
 
   // TF
@@ -79,14 +80,11 @@ int
   rosbag::View view;
   rosbag::View::iterator view_it;
 
-  try
-  {
+  try {
     bag.open(argv[1], rosbag::bagmode::Read);
-  } 
-  catch (rosbag::BagException) 
-  {
+  } catch (rosbag::BagException) {
     std::cerr << "Error opening file " << argv[1] << std::endl;
-    return (-1);
+    return -1;
   }
 
   view.addQuery(bag, rosbag::TypeQuery("sensor_msgs/PointCloud2"));
@@ -96,70 +94,63 @@ int
 
   std::string output_dir = std::string(argv[3]);
   boost::filesystem::path outpath(output_dir);
-  if (!boost::filesystem::exists (outpath))
-  {
-    if (!boost::filesystem::create_directories (outpath))
-    {
+  if (!boost::filesystem::exists(outpath)) {
+    if (!boost::filesystem::create_directories(outpath)) {
       std::cerr << "Error creating directory " << output_dir << std::endl;
-      return (-1);
+      return -1;
     }
     std::cerr << "Creating directory " << output_dir << std::endl;
   }
 
   // Add the PointCloud2 handler
-  std::cerr << "Saving recorded sensor_msgs::PointCloud2 messages on topic " << argv[2] << " to " << output_dir << std::endl;
+  std::cerr << "Saving recorded sensor_msgs::PointCloud2 messages on topic " << argv[2] << " to " <<
+    output_dir << std::endl;
 
   PointCloud cloud_t;
   ros::Duration r(0.001);
   // Loop over the whole bag file
-  while (view_it != view.end ())
-  {
+  while (view_it != view.end()) {
     // Handle TF messages first
     tf::tfMessage::ConstPtr tf = view_it->instantiate<tf::tfMessage>();
-    if (tf != NULL)
-    {
+    if (tf != NULL) {
       tf_broadcaster.sendTransform(tf->transforms);
       ros::spinOnce();
       r.sleep();
-    }
-    else
-    {
+    } else {
       // Get the PointCloud2 message
       PointCloudConstPtr cloud = view_it->instantiate<PointCloud>();
-      if (cloud == NULL)
-      {
+      if (cloud == NULL) {
         ++view_it;
         continue;
       }
 
       // If a target_frame was specified
-      if(argc > 4)
-      {
+      if (argc > 4) {
         // Transform it
-        if (!pcl_ros::transformPointCloud (argv[4], *cloud, cloud_t, tf_listener))
-        {
+        if (!pcl_ros::transformPointCloud(argv[4], *cloud, cloud_t, tf_listener)) {
           ++view_it;
           continue;
         }
-      }
-      else
-      {
+      } else {
         // Else, don't transform it
         cloud_t = *cloud;
       }
 
-      std::cerr << "Got " << cloud_t.width * cloud_t.height << " data points in frame " << cloud_t.header.frame_id << " with the following fields: " << pcl::getFieldsList (cloud_t) << std::endl;
+      std::cerr << "Got " << cloud_t.width * cloud_t.height << " data points in frame " <<
+        cloud_t.header.frame_id << " with the following fields: " << pcl::getFieldsList(cloud_t) <<
+        std::endl;
 
       std::stringstream ss;
       ss << output_dir << "/" << cloud_t.header.stamp << ".pcd";
       std::cerr << "Data saved to " << ss.str() << std::endl;
-      pcl::io::savePCDFile (ss.str (), cloud_t, Eigen::Vector4f::Zero (),
+      pcl::io::savePCDFile(
+        ss.str(), cloud_t, Eigen::Vector4f::Zero(),
         Eigen::Quaternionf::Identity(), true);
     }
     // Increment the iterator
     ++view_it;
   }
 
-  return (0);
+  return 0;
 }
 /* ]--- */

pcl_ros/tools/convert_pcd_to_image.cpp

@@ -62,8 +62,7 @@ main (int argc, char **argv)
   ros::NodeHandle nh;
   ros::Publisher image_pub = nh.advertise<sensor_msgs::Image>("output", 1);
 
-  if (argc != 2)
-  {
+  if (argc != 2) {
     std::cout << "usage:\n" << argv[0] << " cloud.pcd" << std::endl;
     return 1;
   }
@@ -72,25 +71,22 @@ main (int argc, char **argv)
   sensor_msgs::PointCloud2 cloud;
   pcl::io::loadPCDFile(std::string(argv[1]), cloud);
 
-  try
-  {
+  try {
     pcl::toROSMsg(cloud, image);  //convert the cloud
-  }
-  catch (std::runtime_error &e)
-  {
-    ROS_ERROR_STREAM("Error in converting cloud to image message: "
-                            << e.what());
+  } catch (std::runtime_error & e) {
+    ROS_ERROR_STREAM(
+      "Error in converting cloud to image message: " <<
+        e.what());
     return 1; //fail!
   }
   ros::Rate loop_rate(5);
-  while (nh.ok ())
-  {
+  while (nh.ok()) {
     image_pub.publish(image);
     ros::spinOnce();
     loop_rate.sleep();
   }
 
-  return (0);
+  return 0;
 }
 
 /* ]--- */

pcl_ros/tools/convert_pointcloud_to_image.cpp

@@ -56,26 +56,25 @@ public:
   void
   cloud_cb(const sensor_msgs::PointCloud2ConstPtr & cloud)
   {
-    if (cloud->height <= 1)
-    {
+    if (cloud->height <= 1) {
       ROS_ERROR("Input point cloud is not organized, ignoring!");
       return;
     }
-    try
-    {
+    try {
       pcl::toROSMsg(*cloud, image_);  //convert the cloud
       image_.header = cloud->header;
       image_pub_.publish(image_);  //publish our cloud image
+    } catch (std::runtime_error & e) {
+      ROS_ERROR_STREAM(
+        "Error in converting cloud to image message: " <<
+          e.what());
     }
-    catch (std::runtime_error &e)
+  }
+  PointCloudToImage()
+  : cloud_topic_("input"), image_topic_("output")
   {
-      ROS_ERROR_STREAM("Error in converting cloud to image message: "
-                        << e.what());
-    }
-  }
-  PointCloudToImage () : cloud_topic_("input"),image_topic_("output")
-  {
-    sub_ = nh_.subscribe (cloud_topic_, 30,
+    sub_ = nh_.subscribe(
+      cloud_topic_, 30,
       &PointCloudToImage::cloud_cb, this);
     image_pub_ = nh_.advertise<sensor_msgs::Image>(image_topic_, 30);
 
@@ -85,6 +84,7 @@ public:
     ROS_INFO_STREAM("Listening for incoming data on topic " << r_ct);
     ROS_INFO_STREAM("Publishing image on topic " << r_it);
   }
+
 private:
   ros::NodeHandle nh_;
   sensor_msgs::Image image_; //cache the image message

pcl_ros/tools/pcd_to_pointcloud.cpp

@@ -64,8 +64,8 @@ class PCDGenerator
   string tf_frame_;
   ros::NodeHandle nh_;
   ros::NodeHandle private_nh_;
-  public:
 
+public:
   // ROS messages
   sensor_msgs::PointCloud2 cloud_;
 
@@ -75,14 +75,17 @@ class PCDGenerator
   pcl_ros::Publisher<sensor_msgs::PointCloud2> pub_;
 
   ////////////////////////////////////////////////////////////////////////////////
-    PCDGenerator () : tf_frame_ ("/base_link"), private_nh_("~")
+  PCDGenerator()
+  : tf_frame_("/base_link"), private_nh_("~")
   {
     // Maximum number of outgoing messages to be queued for delivery to subscribers = 1
 
     cloud_topic_ = "cloud_pcd";
     pub_.advertise(nh_, cloud_topic_.c_str(), 1);
     private_nh_.param("frame_id", tf_frame_, std::string("/base_link"));
-      ROS_INFO ("Publishing data on topic %s with frame_id %s.", nh_.resolveName (cloud_topic_).c_str (), tf_frame_.c_str());
+    ROS_INFO(
+      "Publishing data on topic %s with frame_id %s.", nh_.resolveName(
+        cloud_topic_).c_str(), tf_frame_.c_str());
   }
 
   ////////////////////////////////////////////////////////////////////////////////
@@ -90,10 +93,11 @@ class PCDGenerator
   int
   start()
   {
-      if (file_name_ == "" || pcl::io::loadPCDFile (file_name_, cloud_) == -1)
-        return (-1);
+    if (file_name_ == "" || pcl::io::loadPCDFile(file_name_, cloud_) == -1) {
+      return -1;
+    }
     cloud_.header.frame_id = tf_frame_;
-      return (0);
+    return 0;
   }
 
   ////////////////////////////////////////////////////////////////////////////////
@@ -103,18 +107,16 @@ class PCDGenerator
     int nr_points = cloud_.width * cloud_.height;
     string fields_list = pcl::getFieldsList(cloud_);
     double interval = wait_ * 1e+6;
-      while (nh_.ok ())
-      {
-        ROS_DEBUG_ONCE ("Publishing data with %d points (%s) on topic %s in frame %s.", nr_points, fields_list.c_str (), nh_.resolveName (cloud_topic_).c_str (), cloud_.header.frame_id.c_str ());
+    while (nh_.ok()) {
+      ROS_DEBUG_ONCE(
+        "Publishing data with %d points (%s) on topic %s in frame %s.", nr_points,
+        fields_list.c_str(), nh_.resolveName(cloud_topic_).c_str(), cloud_.header.frame_id.c_str());
       cloud_.header.stamp = ros::Time::now();
 
-        if (pub_.getNumSubscribers () > 0)
-        {
+      if (pub_.getNumSubscribers() > 0) {
         ROS_DEBUG("Publishing data to %d subscribers.", pub_.getNumSubscribers());
         pub_.publish(cloud_);
-        }
-        else
-        {
+      } else {
         // check once a second if there is any subscriber
         ros::Duration(1).sleep();
         continue;
@@ -122,14 +124,13 @@ class PCDGenerator
 
       std::this_thread::sleep_for(std::chrono::microseconds(static_cast<uint32_t>(interval)));
 
-        if (interval == 0)	// We only publish once if a 0 seconds interval is given
-				{
+      if (interval == 0) {      // We only publish once if a 0 seconds interval is given
         // Give subscribers 3 seconds until point cloud decays... a little ugly!
         ros::Duration(3.0).sleep();
         break;
       }
     }
-      return (true);
+    return true;
   }
 
 
@@ -139,10 +140,10 @@ class PCDGenerator
 int
 main(int argc, char ** argv)
 {
-  if (argc < 2)
-  {
-    std::cerr << "Syntax is: " << argv[0] << " <file.pcd> [publishing_interval (in seconds)]" << std::endl;
-    return (-1);
+  if (argc < 2) {
+    std::cerr << "Syntax is: " << argv[0] << " <file.pcd> [publishing_interval (in seconds)]" <<
+      std::endl;
+    return -1;
   }
 
   ros::init(argc, argv, "pcd_to_pointcloud");
@@ -150,23 +151,21 @@ int
   PCDGenerator c;
   c.file_name_ = string(argv[1]);
   // check if publishing interval is given
-  if (argc == 2)
-	{
+  if (argc == 2) {
     c.wait_ = 0;
-	}
-	else
-	{
+  } else {
     c.wait_ = atof(argv[2]);
   }
 
-  if (c.start () == -1)
-  {
+  if (c.start() == -1) {
     ROS_ERROR("Could not load file %s. Exiting.", argv[1]);
-    return (-1);
+    return -1;
   }
-  ROS_INFO ("Loaded a point cloud with %d points (total size is %zu) and the following channels: %s.",  c.cloud_.width * c.cloud_.height, c.cloud_.data.size (), pcl::getFieldsList (c.cloud_).c_str ());
+  ROS_INFO(
+    "Loaded a point cloud with %d points (total size is %zu) and the following channels: %s.",
+    c.cloud_.width * c.cloud_.height, c.cloud_.data.size(), pcl::getFieldsList(c.cloud_).c_str());
   c.spin();
 
-  return (0);
+  return 0;
 }
 /* ]--- */

pcl_ros/tools/pointcloud_to_pcd.cpp

@@ -85,10 +85,12 @@ class PointCloudToPCD
   void
   cloud_cb(const boost::shared_ptr<const pcl::PCLPointCloud2> & cloud)
   {
-      if ((cloud->width * cloud->height) == 0)
+    if ((cloud->width * cloud->height) == 0) {
       return;
+    }
 
-      ROS_INFO ("Received %d data points in frame %s with the following fields: %s",
+    ROS_INFO(
+      "Received %d data points in frame %s with the following fields: %s",
       (int)cloud->width * cloud->height,
       cloud->header.frame_id.c_str(),
       pcl::getFieldsList(*cloud).c_str());
@@ -96,13 +98,20 @@ class PointCloudToPCD
     Eigen::Vector4f v = Eigen::Vector4f::Zero();
     Eigen::Quaternionf q = Eigen::Quaternionf::Identity();
     if (!fixed_frame_.empty()) {
-        if (!tf_buffer_.canTransform (fixed_frame_, cloud->header.frame_id, pcl_conversions::fromPCL (cloud->header.stamp), ros::Duration (3.0))) {
+      if (!tf_buffer_.canTransform(
+          fixed_frame_, cloud->header.frame_id,
+          pcl_conversions::fromPCL(cloud->header.stamp), ros::Duration(3.0)))
+      {
         ROS_WARN("Could not get transform!");
         return;
       }
 
       Eigen::Affine3d transform;
-        transform = tf2::transformToEigen (tf_buffer_.lookupTransform (fixed_frame_, cloud->header.frame_id,  pcl_conversions::fromPCL (cloud->header.stamp)));
+      transform =
+        tf2::transformToEigen(
+        tf_buffer_.lookupTransform(
+          fixed_frame_, cloud->header.frame_id,
+          pcl_conversions::fromPCL(cloud->header.stamp)));
       v = Eigen::Vector4f::Zero();
       v.head<3>() = transform.translation().cast<float>();
       q = transform.rotation().cast<float>();
@@ -113,62 +122,50 @@ class PointCloudToPCD
     ROS_INFO("Data saved to %s", ss.str().c_str());
 
     pcl::PCDWriter writer;
-      if(binary_)
-	{
-	  if(compressed_)
-	    {
+    if (binary_) {
+      if (compressed_) {
         writer.writeBinaryCompressed(ss.str(), *cloud, v, q);
-	    }
-	  else
-	    {
+      } else {
         writer.writeBinary(ss.str(), *cloud, v, q);
       }
-	}
-      else
-	{
+    } else {
       writer.writeASCII(ss.str(), *cloud, v, q, 8);
     }
 
   }
 
   ////////////////////////////////////////////////////////////////////////////////
-    PointCloudToPCD () : binary_(false), compressed_(false), tf_listener_(tf_buffer_)
+  PointCloudToPCD()
+  : binary_(false), compressed_(false), tf_listener_(tf_buffer_)
   {
     // Check if a prefix parameter is defined for output file names.
     ros::NodeHandle priv_nh("~");
-      if (priv_nh.getParam ("prefix", prefix_))
-        {
+    if (priv_nh.getParam("prefix", prefix_)) {
       ROS_INFO_STREAM("PCD file prefix is: " << prefix_);
-        }
-      else if (nh_.getParam ("prefix", prefix_))
-        {
-          ROS_WARN_STREAM ("Non-private PCD prefix parameter is DEPRECATED: "
-                           << prefix_);
+    } else if (nh_.getParam("prefix", prefix_)) {
+      ROS_WARN_STREAM(
+        "Non-private PCD prefix parameter is DEPRECATED: " <<
+          prefix_);
     }
 
     priv_nh.getParam("fixed_frame", fixed_frame_);
     priv_nh.getParam("binary", binary_);
     priv_nh.getParam("compressed", compressed_);
-      if(binary_)
-	{
-	  if(compressed_)
-	    {
+    if (binary_) {
+      if (compressed_) {
         ROS_INFO_STREAM("Saving as binary compressed PCD");
-	    }
-	  else
-	    {
+      } else {
         ROS_INFO_STREAM("Saving as binary PCD");
       }
-	}
-      else
-	{
+    } else {
       ROS_INFO_STREAM("Saving as binary PCD");
     }
 
     cloud_topic_ = "input";
 
     sub_ = nh_.subscribe(cloud_topic_, 1, &PointCloudToPCD::cloud_cb, this);
-      ROS_INFO ("Listening for incoming data on topic %s",
+    ROS_INFO(
+      "Listening for incoming data on topic %s",
       nh_.resolveName(cloud_topic_).c_str());
   }
 };
@@ -182,6 +179,6 @@ int
   PointCloudToPCD b;
   ros::spin();
 
-  return (0);
+  return 0;
 }
 /* ]--- */