ApproximateVoxelGridFilter.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/approximate_voxel_grid.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", "ApproximateVoxelGridFilter",
21  "type_name", "ApproximateVoxelGridFilter",
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 
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 ApproximateVoxelGridFilter::onFinalize()
108 {
109  return RTC::RTC_OK;
110 }
111 */
112 
113 /*
114 RTC::ReturnCode_t ApproximateVoxelGridFilter::onStartup(RTC::UniqueId ec_id)
115 {
116  return RTC::RTC_OK;
117 }
118 */
119 
120 /*
121 RTC::ReturnCode_t ApproximateVoxelGridFilter::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->points.resize(m_original.width*m_original.height);
159  float *src = (float *)m_original.data.get_buffer();
160  for (unsigned int i=0; i<cloud->points.size(); i++){
161  cloud->points[i].x = src[0];
162  cloud->points[i].y = src[1];
163  cloud->points[i].z = src[2];
164  src += 4;
165  }
166 
167  // PCL Processing
168  pcl::ApproximateVoxelGrid<pcl::PointXYZ> sor;
169  sor.setInputCloud (cloud);
170  sor.setLeafSize(m_size, m_size, m_size);
171  sor.filter(*cloud_filtered);
172  if (m_debugLevel > 0){
173  std::cout << cloud->points.size() << " points are reduced to "
174  << cloud_filtered->points.size() << " points" << std::endl;
175  }
176 
177 
178  // PCL -> RTM
179  m_filtered.width = cloud_filtered->points.size();
180  m_filtered.row_step = m_filtered.point_step*m_filtered.width;
181  m_filtered.data.length(m_filtered.height*m_filtered.row_step);
182  m_filtered.tm = m_original.tm;
183  float *dst = (float *)m_filtered.data.get_buffer();
184  for (unsigned int i=0; i<cloud_filtered->points.size(); i++){
185  dst[0] = cloud_filtered->points[i].x;
186  dst[1] = cloud_filtered->points[i].y;
187  dst[2] = cloud_filtered->points[i].z;
188  dst += 4;
189  }
191  }
192 
193  return RTC::RTC_OK;
194 }
195 
196 /*
197 RTC::ReturnCode_t ApproximateVoxelGridFilter::onAborting(RTC::UniqueId ec_id)
198 {
199  return RTC::RTC_OK;
200 }
201 */
202 
203 /*
204 RTC::ReturnCode_t ApproximateVoxelGridFilter::onError(RTC::UniqueId ec_id)
205 {
206  return RTC::RTC_OK;
207 }
208 */
209 
210 /*
211 RTC::ReturnCode_t ApproximateVoxelGridFilter::onReset(RTC::UniqueId ec_id)
212 {
213  return RTC::RTC_OK;
214 }
215 */
216 
217 /*
218 RTC::ReturnCode_t ApproximateVoxelGridFilter::onStateUpdate(RTC::UniqueId ec_id)
219 {
220  return RTC::RTC_OK;
221 }
222 */
223 
224 /*
225 RTC::ReturnCode_t ApproximateVoxelGridFilter::onRateChanged(RTC::UniqueId ec_id)
226 {
227  return RTC::RTC_OK;
228 }
229 */
230 
231 
232 
233 extern "C"
234 {
235 
237  {
239  manager->registerFactory(profile,
240  RTC::Create<ApproximateVoxelGridFilter>,
241  RTC::Delete<ApproximateVoxelGridFilter>);
242  }
243 
244 };
245 
246 
ComponentProfile m_profile
Moving Least Squares Filter.
png_infop png_charpp int png_charpp profile
virtual ~ApproximateVoxelGridFilter()
Destructor.
ApproximateVoxelGridFilter(RTC::Manager *manager)
Constructor.
PointCloudTypes::PointCloud m_original
OutPort< PointCloudTypes::PointCloud > m_filteredOut
png_uint_32 i
coil::Properties & getProperties()
bool addOutPort(const char *name, OutPortBase &outport)
ExecutionContextHandle_t UniqueId
bool bindParameter(const char *param_name, VarType &var, const char *def_val, bool(*trans)(VarType &, const char *)=coil::stringTo)
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id)
PointCloudTypes::PointCloud m_filtered
prop
virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id)
virtual RTC::ReturnCode_t onInitialize()
virtual bool isNew()
virtual bool write(DataType &value)
static const char * spec[]
InPort< PointCloudTypes::PointCloud > m_originalIn
bool addInPort(const char *name, InPortBase &inport)
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id)
void ApproximateVoxelGridFilterInit(RTC::Manager *manager)


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Sat Dec 17 2022 03:52:20