ApproximateVoxelGridFilter.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #include <pcl/io/pcd_io.h>
00011 #include <pcl/point_types.h>
00012 #include <pcl/filters/approximate_voxel_grid.h>
00013 #include "ApproximateVoxelGridFilter.h"
00014 #include "hrpsys/idl/pointcloud.hh"
00015 
00016 // Module specification
00017 // <rtc-template block="module_spec">
00018 static const char* spec[] =
00019   {
00020     "implementation_id", "ApproximateVoxelGridFilter",
00021     "type_name",         "ApproximateVoxelGridFilter",
00022     "description",       "Voxel Grid Filter",
00023     "version",           HRPSYS_PACKAGE_VERSION,
00024     "vendor",            "AIST",
00025     "category",          "example",
00026     "activity_type",     "DataFlowComponent",
00027     "max_instance",      "10",
00028     "language",          "C++",
00029     "lang_type",         "compile",
00030     // Configuration variables
00031     "conf.default.size", "0.01",
00032     "conf.default.debugLevel", "0",
00033 
00034     ""
00035   };
00036 // </rtc-template>
00037 
00038 ApproximateVoxelGridFilter::ApproximateVoxelGridFilter(RTC::Manager* manager)
00039   : RTC::DataFlowComponentBase(manager),
00040     // <rtc-template block="initializer">
00041     m_originalIn("original", m_original),
00042     m_filteredOut("filtered", m_filtered),
00043     // </rtc-template>
00044     dummy(0)
00045 {
00046 }
00047 
00048 ApproximateVoxelGridFilter::~ApproximateVoxelGridFilter()
00049 {
00050 }
00051 
00052 
00053 
00054 RTC::ReturnCode_t ApproximateVoxelGridFilter::onInitialize()
00055 {
00056   //std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
00057   // <rtc-template block="bind_config">
00058   // Bind variables and configuration variable
00059   bindParameter("size", m_size, "0.01");
00060   bindParameter("debugLevel", m_debugLevel, "0");
00061   
00062   // </rtc-template>
00063 
00064   // Registration: InPort/OutPort/Service
00065   // <rtc-template block="registration">
00066   // Set InPort buffers
00067   addInPort("originalIn", m_originalIn);
00068 
00069   // Set OutPort buffer
00070   addOutPort("filteredOut", m_filteredOut);
00071   
00072   // Set service provider to Ports
00073   
00074   // Set service consumers to Ports
00075   
00076   // Set CORBA Service Ports
00077   
00078   // </rtc-template>
00079 
00080   RTC::Properties& prop = getProperties();
00081 
00082   m_filtered.height = 1;
00083   m_filtered.type = "xyz";
00084   m_filtered.fields.length(3);
00085   m_filtered.fields[0].name = "x";
00086   m_filtered.fields[0].offset = 0;
00087   m_filtered.fields[0].data_type = PointCloudTypes::FLOAT32;
00088   m_filtered.fields[0].count = 4;
00089   m_filtered.fields[1].name = "y";
00090   m_filtered.fields[1].offset = 4;
00091   m_filtered.fields[1].data_type = PointCloudTypes::FLOAT32;
00092   m_filtered.fields[1].count = 4;
00093   m_filtered.fields[2].name = "z";
00094   m_filtered.fields[2].offset = 8;
00095   m_filtered.fields[2].data_type = PointCloudTypes::FLOAT32;
00096   m_filtered.fields[2].count = 4;
00097   m_filtered.is_bigendian = false;
00098   m_filtered.point_step = 16;
00099   m_filtered.is_dense = true;
00100 
00101   return RTC::RTC_OK;
00102 }
00103 
00104 
00105 
00106 /*
00107 RTC::ReturnCode_t ApproximateVoxelGridFilter::onFinalize()
00108 {
00109   return RTC::RTC_OK;
00110 }
00111 */
00112 
00113 /*
00114 RTC::ReturnCode_t ApproximateVoxelGridFilter::onStartup(RTC::UniqueId ec_id)
00115 {
00116   return RTC::RTC_OK;
00117 }
00118 */
00119 
00120 /*
00121 RTC::ReturnCode_t ApproximateVoxelGridFilter::onShutdown(RTC::UniqueId ec_id)
00122 {
00123   return RTC::RTC_OK;
00124 }
00125 */
00126 
00127 RTC::ReturnCode_t ApproximateVoxelGridFilter::onActivated(RTC::UniqueId ec_id)
00128 {
00129   std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl;
00130   return RTC::RTC_OK;
00131 }
00132 
00133 RTC::ReturnCode_t ApproximateVoxelGridFilter::onDeactivated(RTC::UniqueId ec_id)
00134 {
00135   std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl;
00136   return RTC::RTC_OK;
00137 }
00138 
00139 RTC::ReturnCode_t ApproximateVoxelGridFilter::onExecute(RTC::UniqueId ec_id)
00140 {
00141   if (m_debugLevel > 0){
00142    std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl;
00143   }
00144 
00145   if (m_originalIn.isNew()){
00146     m_originalIn.read();
00147 
00148     if (!m_size){
00149         m_filtered = m_original;
00150         m_filteredOut.write();
00151         return RTC::RTC_OK;
00152     }
00153 
00154     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
00155     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
00156 
00157     // RTM -> PCL
00158     cloud->points.resize(m_original.width*m_original.height);
00159     float *src = (float *)m_original.data.get_buffer();
00160     for (unsigned int i=0; i<cloud->points.size(); i++){
00161       cloud->points[i].x = src[0];
00162       cloud->points[i].y = src[1];
00163       cloud->points[i].z = src[2];
00164       src += 4;
00165     }
00166     
00167     // PCL Processing 
00168     pcl::ApproximateVoxelGrid<pcl::PointXYZ> sor;
00169     sor.setInputCloud (cloud);
00170     sor.setLeafSize(m_size, m_size, m_size);
00171     sor.filter(*cloud_filtered);
00172     if (m_debugLevel > 0){
00173       std::cout << cloud->points.size() << " points are reduced to "
00174                 << cloud_filtered->points.size() << " points" << std::endl;
00175     }
00176       
00177 
00178     // PCL -> RTM
00179     m_filtered.width = cloud_filtered->points.size();
00180     m_filtered.row_step = m_filtered.point_step*m_filtered.width;
00181     m_filtered.data.length(m_filtered.height*m_filtered.row_step);
00182     m_filtered.tm = m_original.tm;
00183     float *dst = (float *)m_filtered.data.get_buffer();
00184     for (unsigned int i=0; i<cloud_filtered->points.size(); i++){
00185       dst[0] = cloud_filtered->points[i].x;
00186       dst[1] = cloud_filtered->points[i].y;
00187       dst[2] = cloud_filtered->points[i].z;
00188       dst += 4;
00189     }
00190     m_filteredOut.write();
00191   }
00192 
00193   return RTC::RTC_OK;
00194 }
00195 
00196 /*
00197 RTC::ReturnCode_t ApproximateVoxelGridFilter::onAborting(RTC::UniqueId ec_id)
00198 {
00199   return RTC::RTC_OK;
00200 }
00201 */
00202 
00203 /*
00204 RTC::ReturnCode_t ApproximateVoxelGridFilter::onError(RTC::UniqueId ec_id)
00205 {
00206   return RTC::RTC_OK;
00207 }
00208 */
00209 
00210 /*
00211 RTC::ReturnCode_t ApproximateVoxelGridFilter::onReset(RTC::UniqueId ec_id)
00212 {
00213   return RTC::RTC_OK;
00214 }
00215 */
00216 
00217 /*
00218 RTC::ReturnCode_t ApproximateVoxelGridFilter::onStateUpdate(RTC::UniqueId ec_id)
00219 {
00220   return RTC::RTC_OK;
00221 }
00222 */
00223 
00224 /*
00225 RTC::ReturnCode_t ApproximateVoxelGridFilter::onRateChanged(RTC::UniqueId ec_id)
00226 {
00227   return RTC::RTC_OK;
00228 }
00229 */
00230 
00231 
00232 
00233 extern "C"
00234 {
00235 
00236   void ApproximateVoxelGridFilterInit(RTC::Manager* manager)
00237   {
00238     RTC::Properties profile(spec);
00239     manager->registerFactory(profile,
00240                              RTC::Create<ApproximateVoxelGridFilter>,
00241                              RTC::Delete<ApproximateVoxelGridFilter>);
00242   }
00243 
00244 };
00245 
00246 


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:54