euclidean_cluster_extraction_nodelet.h
Go to the documentation of this file.
00001 // -*- mode: C++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2013, Ryohei Ueda and JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #ifndef JSK_PCL_ROS_EUCLIDEAN_CLUSTER_EXTRACTION_NODELET_H_
00037 #define JSK_PCL_ROS_EUCLIDEAN_CLUSTER_EXTRACTION_NODELET_H_
00038 
00039 #include <ros/ros.h>
00040 #include <ros/names.h>
00041 
00042 #include <std_msgs/ColorRGBA.h>
00043 
00044 #include <dynamic_reconfigure/server.h>
00045 
00046 #include <pcl_ros/pcl_nodelet.h>
00047 
00048 #include <pcl/point_types.h>
00049 #include <pcl/impl/point_types.hpp>
00050 #include <pcl/kdtree/kdtree_flann.h>
00051 #include <pcl/segmentation/sac_segmentation.h>
00052 #include <pcl/segmentation/extract_clusters.h>
00053 #include <pcl/filters/extract_indices.h>
00054 #include <pcl/common/centroid.h>
00055 
00056 #include "jsk_recognition_msgs/ClusterPointIndices.h"
00057 #include "jsk_pcl_ros/EuclideanSegment.h"
00058 #include "jsk_recognition_msgs/Int32Stamped.h"
00059 
00060 #include "jsk_pcl_ros/EuclideanClusteringConfig.h"
00061 #include <diagnostic_updater/diagnostic_updater.h>
00062 #include <diagnostic_updater/publisher.h>
00063 #include <Eigen/StdVector>
00064 #include "jsk_pcl_ros/pcl_util.h"
00065 #include "jsk_pcl_ros/pcl_conversion_util.h"
00066 #include <jsk_topic_tools/vital_checker.h>
00067 #include <jsk_topic_tools/time_accumulator.h>
00068 
00069 #include <jsk_topic_tools/connection_based_nodelet.h>
00070 
00071 namespace jsk_pcl_ros
00072 {
00073   class EuclideanClustering : public jsk_topic_tools::ConnectionBasedNodelet
00074   {
00075   public:
00076     typedef jsk_pcl_ros::EuclideanClusteringConfig Config;
00077     typedef std::vector<Eigen::Vector4f,
00078                         Eigen::aligned_allocator<Eigen::Vector4f> >
00079     Vector4fVector;
00080     
00081   protected:
00082     boost::shared_ptr <dynamic_reconfigure::Server<Config> > srv_;
00083     boost::mutex mutex_;
00084    
00085     void configCallback (Config &config, uint32_t level);
00086     ros::Publisher result_pub_;
00087     ros::Subscriber sub_input_;
00088     ros::Publisher cluster_num_pub_;
00089 
00090     ros::ServiceServer service_;
00091 
00092     double tolerance;
00093     double label_tracking_tolerance;
00094     int minsize_;
00095     int maxsize_;
00096     
00097     jsk_pcl_ros::TimeredDiagnosticUpdater::Ptr diagnostic_updater_;
00098     jsk_topic_tools::VitalChecker::Ptr vital_checker_;
00099     jsk_topic_tools::TimeAccumulator segmentation_acc_;
00100     jsk_topic_tools::TimeAccumulator kdtree_acc_;
00101     Counter cluster_counter_;
00102     
00103     // the list of COGs of each cluster
00104     Vector4fVector cogs_;
00105     
00106     virtual void onInit();
00107     virtual void extract(const sensor_msgs::PointCloud2ConstPtr &input);
00108     bool serviceCallback(jsk_pcl_ros::EuclideanSegment::Request &req,
00109                          jsk_pcl_ros::EuclideanSegment::Response &res);
00110     void updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat);
00111     virtual std::vector<pcl::PointIndices> pivotClusterIndices(
00112       std::vector<int>& pivot_table,
00113       std::vector<pcl::PointIndices>& cluster_indices);
00114       
00115     virtual std::vector<int> buildLabelTrackingPivotTable(
00116       double* D,
00117       Vector4fVector cogs,
00118       Vector4fVector new_cogs,
00119       double label_tracking_tolerance);
00120     
00121     virtual void computeDistanceMatrix(
00122       double* D,
00123       Vector4fVector& old_cogs,
00124       Vector4fVector& new_cogs);
00125       
00126     virtual void
00127     computeCentroidsOfClusters(Vector4fVector& ret,
00128                                pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
00129                                std::vector<pcl::PointIndices> cluster_indices);
00130 
00131     virtual void subscribe();
00132     virtual void unsubscribe();
00133   };
00134     
00135 }
00136 
00137 #endif


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Wed Sep 16 2015 04:36:47