VoxelGridFilter.cpp
Go to the documentation of this file.
1 // -*- C++ -*-
10 #include <pcl/io/pcd_io.h>
11 #include <pcl/point_types.h>
12 #include <pcl/filters/voxel_grid.h>
13 #include "VoxelGridFilter.h"
14 #include "hrpsys/idl/pointcloud.hh"
15 
16 // Module specification
17 // <rtc-template block="module_spec">
18 static const char* spec[] =
19  {
20  "implementation_id", "VoxelGridFilter",
21  "type_name", "VoxelGridFilter",
22  "description", "Voxel Grid Filter",
23  "version", HRPSYS_PACKAGE_VERSION,
24  "vendor", "AIST",
25  "category", "example",
26  "activity_type", "DataFlowComponent",
27  "max_instance", "10",
28  "language", "C++",
29  "lang_type", "compile",
30  // Configuration variables
31  "conf.default.size", "0.01",
32  "conf.default.debugLevel", "0",
33 
34  ""
35  };
36 // </rtc-template>
37 
39  : RTC::DataFlowComponentBase(manager),
40  // <rtc-template block="initializer">
41  m_originalIn("original", m_original),
42  m_filteredOut("filtered", m_filtered),
43  // </rtc-template>
44  dummy(0)
45 {
46 }
47 
49 {
50 }
51 
52 
53 
54 RTC::ReturnCode_t VoxelGridFilter::onInitialize()
55 {
56  //std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
57  // <rtc-template block="bind_config">
58  // Bind variables and configuration variable
59  bindParameter("size", m_size, "0.01");
60  bindParameter("debugLevel", m_debugLevel, "0");
61 
62  // </rtc-template>
63 
64  // Registration: InPort/OutPort/Service
65  // <rtc-template block="registration">
66  // Set InPort buffers
67  addInPort("originalIn", m_originalIn);
68 
69  // Set OutPort buffer
70  addOutPort("filteredOut", m_filteredOut);
71 
72  // Set service provider to Ports
73 
74  // Set service consumers to Ports
75 
76  // Set CORBA Service Ports
77 
78  // </rtc-template>
79 
81 
82  m_filtered.height = 1;
83  m_filtered.type = "xyz";
84  m_filtered.fields.length(3);
85  m_filtered.fields[0].name = "x";
86  m_filtered.fields[0].offset = 0;
87  m_filtered.fields[0].data_type = PointCloudTypes::FLOAT32;
88  m_filtered.fields[0].count = 4;
89  m_filtered.fields[1].name = "y";
90  m_filtered.fields[1].offset = 4;
91  m_filtered.fields[1].data_type = PointCloudTypes::FLOAT32;
92  m_filtered.fields[1].count = 4;
93  m_filtered.fields[2].name = "z";
94  m_filtered.fields[2].offset = 8;
95  m_filtered.fields[2].data_type = PointCloudTypes::FLOAT32;
96  m_filtered.fields[2].count = 4;
97  m_filtered.is_bigendian = false;
98  m_filtered.point_step = 16;
99  m_filtered.is_dense = true;
100 
101  return RTC::RTC_OK;
102 }
103 
104 
105 
106 /*
107 RTC::ReturnCode_t VoxelGridFilter::onFinalize()
108 {
109  return RTC::RTC_OK;
110 }
111 */
112 
113 /*
114 RTC::ReturnCode_t VoxelGridFilter::onStartup(RTC::UniqueId ec_id)
115 {
116  return RTC::RTC_OK;
117 }
118 */
119 
120 /*
121 RTC::ReturnCode_t VoxelGridFilter::onShutdown(RTC::UniqueId ec_id)
122 {
123  return RTC::RTC_OK;
124 }
125 */
126 
128 {
129  std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl;
130  return RTC::RTC_OK;
131 }
132 
134 {
135  std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl;
136  return RTC::RTC_OK;
137 }
138 
140 {
141  if (m_debugLevel > 0){
142  std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl;
143  }
144 
145  if (m_originalIn.isNew()){
146  m_originalIn.read();
147 
148  if (!m_size){
151  return RTC::RTC_OK;
152  }
153 
154  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
155  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
156 
157  // RTM -> PCL
158  cloud->is_dense = m_original.is_dense;
159  cloud->points.resize(m_original.width*m_original.height);
160  float *src = (float *)m_original.data.get_buffer();
161  for (unsigned int i=0; i<cloud->points.size(); i++){
162  cloud->points[i].x = src[0];
163  cloud->points[i].y = src[1];
164  cloud->points[i].z = src[2];
165  src += 4;
166  }
167 
168  // PCL Processing
169  pcl::VoxelGrid<pcl::PointXYZ> sor;
170  sor.setInputCloud (cloud);
171  sor.setLeafSize(m_size, m_size, m_size);
172  sor.filter(*cloud_filtered);
173 
174  if (m_debugLevel > 0){
175  std::cout << "original:" << cloud->points.size() << ", filterd:" << cloud_filtered->points.size() << std::endl;
176  }
177 
178 
179  // PCL -> RTM
180  m_filtered.width = cloud_filtered->points.size();
181  m_filtered.height = 1;
182  m_filtered.row_step = m_filtered.point_step*m_filtered.width;
183  m_filtered.data.length(m_filtered.height*m_filtered.row_step);
184  float *dst = (float *)m_filtered.data.get_buffer();
185  for (unsigned int i=0; i<cloud_filtered->points.size(); i++){
186  dst[0] = cloud_filtered->points[i].x;
187  dst[1] = cloud_filtered->points[i].y;
188  dst[2] = cloud_filtered->points[i].z;
189  dst += 4;
190  }
192  }
193 
194  return RTC::RTC_OK;
195 }
196 
197 /*
198 RTC::ReturnCode_t VoxelGridFilter::onAborting(RTC::UniqueId ec_id)
199 {
200  return RTC::RTC_OK;
201 }
202 */
203 
204 /*
205 RTC::ReturnCode_t VoxelGridFilter::onError(RTC::UniqueId ec_id)
206 {
207  return RTC::RTC_OK;
208 }
209 */
210 
211 /*
212 RTC::ReturnCode_t VoxelGridFilter::onReset(RTC::UniqueId ec_id)
213 {
214  return RTC::RTC_OK;
215 }
216 */
217 
218 /*
219 RTC::ReturnCode_t VoxelGridFilter::onStateUpdate(RTC::UniqueId ec_id)
220 {
221  return RTC::RTC_OK;
222 }
223 */
224 
225 /*
226 RTC::ReturnCode_t VoxelGridFilter::onRateChanged(RTC::UniqueId ec_id)
227 {
228  return RTC::RTC_OK;
229 }
230 */
231 
232 
233 
234 extern "C"
235 {
236 
238  {
240  manager->registerFactory(profile,
241  RTC::Create<VoxelGridFilter>,
242  RTC::Delete<VoxelGridFilter>);
243  }
244 
245 };
246 
247 
ComponentProfile m_profile
png_infop png_charpp int png_charpp profile
VoxelGridFilter(RTC::Manager *manager)
Constructor.
virtual RTC::ReturnCode_t onInitialize()
virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id)
virtual ~VoxelGridFilter()
Destructor.
Moving Least Squares Filter.
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id)
virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id)
png_uint_32 i
coil::Properties & getProperties()
bool addOutPort(const char *name, OutPortBase &outport)
static const char * spec[]
ExecutionContextHandle_t UniqueId
bool bindParameter(const char *param_name, VarType &var, const char *def_val, bool(*trans)(VarType &, const char *)=coil::stringTo)
InPort< PointCloudTypes::PointCloud > m_originalIn
prop
OutPort< PointCloudTypes::PointCloud > m_filteredOut
PointCloudTypes::PointCloud m_original
virtual bool isNew()
void VoxelGridFilterInit(RTC::Manager *manager)
virtual bool write(DataType &value)
bool addInPort(const char *name, InPortBase &inport)
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
PointCloudTypes::PointCloud m_filtered


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Thu May 6 2021 02:41:51