VoxelGridFilter.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/voxel_grid.h>
00013 #include "VoxelGridFilter.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", "VoxelGridFilter",
00021     "type_name",         "VoxelGridFilter",
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 VoxelGridFilter::VoxelGridFilter(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 VoxelGridFilter::~VoxelGridFilter()
00049 {
00050 }
00051 
00052 
00053 
00054 RTC::ReturnCode_t VoxelGridFilter::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 VoxelGridFilter::onFinalize()
00108 {
00109   return RTC::RTC_OK;
00110 }
00111 */
00112 
00113 /*
00114 RTC::ReturnCode_t VoxelGridFilter::onStartup(RTC::UniqueId ec_id)
00115 {
00116   return RTC::RTC_OK;
00117 }
00118 */
00119 
00120 /*
00121 RTC::ReturnCode_t VoxelGridFilter::onShutdown(RTC::UniqueId ec_id)
00122 {
00123   return RTC::RTC_OK;
00124 }
00125 */
00126 
00127 RTC::ReturnCode_t VoxelGridFilter::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 VoxelGridFilter::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 VoxelGridFilter::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->is_dense = m_original.is_dense;
00159     cloud->points.resize(m_original.width*m_original.height);
00160     float *src = (float *)m_original.data.get_buffer();
00161     for (unsigned int i=0; i<cloud->points.size(); i++){
00162       cloud->points[i].x = src[0];
00163       cloud->points[i].y = src[1];
00164       cloud->points[i].z = src[2];
00165       src += 4;
00166     }
00167     
00168     // PCL Processing 
00169     pcl::VoxelGrid<pcl::PointXYZ> sor;
00170     sor.setInputCloud (cloud);
00171     sor.setLeafSize(m_size, m_size, m_size);
00172     sor.filter(*cloud_filtered);
00173 
00174     if (m_debugLevel > 0){
00175       std::cout << "original:" << cloud->points.size() << ", filterd:" << cloud_filtered->points.size() << std::endl;
00176     }
00177 
00178 
00179     // PCL -> RTM
00180     m_filtered.width = cloud_filtered->points.size();
00181     m_filtered.height = 1;
00182     m_filtered.row_step = m_filtered.point_step*m_filtered.width;
00183     m_filtered.data.length(m_filtered.height*m_filtered.row_step);
00184     float *dst = (float *)m_filtered.data.get_buffer();
00185     for (unsigned int i=0; i<cloud_filtered->points.size(); i++){
00186       dst[0] = cloud_filtered->points[i].x;
00187       dst[1] = cloud_filtered->points[i].y;
00188       dst[2] = cloud_filtered->points[i].z;
00189       dst += 4;
00190     }
00191     m_filteredOut.write();
00192   }
00193 
00194   return RTC::RTC_OK;
00195 }
00196 
00197 /*
00198 RTC::ReturnCode_t VoxelGridFilter::onAborting(RTC::UniqueId ec_id)
00199 {
00200   return RTC::RTC_OK;
00201 }
00202 */
00203 
00204 /*
00205 RTC::ReturnCode_t VoxelGridFilter::onError(RTC::UniqueId ec_id)
00206 {
00207   return RTC::RTC_OK;
00208 }
00209 */
00210 
00211 /*
00212 RTC::ReturnCode_t VoxelGridFilter::onReset(RTC::UniqueId ec_id)
00213 {
00214   return RTC::RTC_OK;
00215 }
00216 */
00217 
00218 /*
00219 RTC::ReturnCode_t VoxelGridFilter::onStateUpdate(RTC::UniqueId ec_id)
00220 {
00221   return RTC::RTC_OK;
00222 }
00223 */
00224 
00225 /*
00226 RTC::ReturnCode_t VoxelGridFilter::onRateChanged(RTC::UniqueId ec_id)
00227 {
00228   return RTC::RTC_OK;
00229 }
00230 */
00231 
00232 
00233 
00234 extern "C"
00235 {
00236 
00237   void VoxelGridFilterInit(RTC::Manager* manager)
00238   {
00239     RTC::Properties profile(spec);
00240     manager->registerFactory(profile,
00241                              RTC::Create<VoxelGridFilter>,
00242                              RTC::Delete<VoxelGridFilter>);
00243   }
00244 
00245 };
00246 
00247 


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