PCDLoader.cpp
Go to the documentation of this file.
1 // -*- C++ -*-
10 #include <pcl/point_types.h>
11 #include <pcl/filters/statistical_outlier_removal.h>
12 #include "PCDLoader.h"
13 #include "hrpsys/idl/pointcloud.hh"
14 
15 // Module specification
16 // <rtc-template block="module_spec">
17 static const char* spec[] =
18  {
19  "implementation_id", "PCDLoader",
20  "type_name", "PCDLoader",
21  "description", "PCD file loader",
22  "version", HRPSYS_PACKAGE_VERSION,
23  "vendor", "AIST",
24  "category", "example",
25  "activity_type", "DataFlowComponent",
26  "max_instance", "10",
27  "language", "C++",
28  "lang_type", "compile",
29  // Configuration variables
30  "conf.default.path", "",
31  "conf.default.fields","XYZ",
32 
33  ""
34  };
35 // </rtc-template>
36 
37 PCDLoader::PCDLoader(RTC::Manager* manager)
38  : RTC::DataFlowComponentBase(manager),
39  // <rtc-template block="initializer">
40  m_offsetIn("offset", m_offset),
41  m_isOutputOut("isOutput", m_isOutput),
42  m_cloudOut("cloud", m_cloud),
43  // </rtc-template>
44  m_PCDLoaderServicePort("PCDLoaderService"),
45  dummy(0)
46 {
47 }
48 
49 PCDLoader::~PCDLoader()
50 {
51 }
52 
53 
54 
55 RTC::ReturnCode_t PCDLoader::onInitialize()
56 {
57  //std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
58  // <rtc-template block="bind_config">
59  // Bind variables and configuration variable
60  bindParameter("path", m_path, "");
61  bindParameter("fields", m_fields, "XYZ");
62 
63  // </rtc-template>
64 
65  // Registration: InPort/OutPort/Service
66  // <rtc-template block="registration">
67  // Set InPort buffers
68  addInPort("offsetIn", m_offsetIn);
69 
70  // Set OutPort buffer
71  addOutPort("isOutputOut", m_isOutputOut);
72  addOutPort("cloudOut", m_cloudOut);
73 
74  // Set service provider to Ports
75  m_PCDLoaderServicePort.registerProvider("service0", "PCDLoaderService", m_service0);
76  addPort(m_PCDLoaderServicePort);
77  m_service0.setComp(this);
78  m_isOutput.data = true;
79 
80  // Set service consumers to Ports
81 
82  // Set CORBA Service Ports
83 
84  // </rtc-template>
85 
86  RTC::Properties& prop = getProperties();
87 
88  return RTC::RTC_OK;
89 }
90 
91 void PCDLoader::setCloudXYZ(PointCloudTypes::PointCloud& cloud, const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_raw)
92 {
93  int npoint = cloud_raw->points.size();
94 
95  cloud.type = "xyz";
96  cloud.fields.length(3);
97  cloud.fields[0].name = "x";
98  cloud.fields[0].offset = 0;
99  cloud.fields[0].data_type = PointCloudTypes::FLOAT32;
100  cloud.fields[0].count = 4;
101  cloud.fields[1].name = "y";
102  cloud.fields[1].offset = 4;
103  cloud.fields[1].data_type = PointCloudTypes::FLOAT32;
104  cloud.fields[1].count = 4;
105  cloud.fields[2].name = "z";
106  cloud.fields[2].offset = 8;
107  cloud.fields[2].data_type = PointCloudTypes::FLOAT32;
108  cloud.fields[2].count = 4;
109  cloud.is_bigendian = false;
110  cloud.point_step = 16;
111  cloud.width = cloud_raw->width;
112  cloud.height = cloud_raw->height;
113  cloud.data.length(npoint*cloud.point_step);
114  cloud.row_step = cloud.width*cloud.point_step;
115  cloud.is_dense = cloud_raw->is_dense;
116  float *ptr = (float *)cloud.data.get_buffer();
117  std::cout << "npoint = " << npoint << std::endl;
118  for (int i=0; i<npoint; i++){
119  ptr[0] = cloud_raw->points[i].x;
120  ptr[1] = cloud_raw->points[i].y;
121  ptr[2] = cloud_raw->points[i].z;
122  ptr += 4;
123  }
124 }
125 
126 void PCDLoader::setCloudXYZRGB(PointCloudTypes::PointCloud& cloud, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_raw)
127 {
128  int npoint = cloud_raw->points.size();
129 
130  cloud.type = "xyzrgb";
131  cloud.fields.length(6);
132  cloud.fields[0].name = "x";
133  cloud.fields[0].offset = 0;
134  cloud.fields[0].data_type = PointCloudTypes::FLOAT32;
135  cloud.fields[0].count = 4;
136  cloud.fields[1].name = "y";
137  cloud.fields[1].offset = 4;
138  cloud.fields[1].data_type = PointCloudTypes::FLOAT32;
139  cloud.fields[1].count = 4;
140  cloud.fields[2].name = "z";
141  cloud.fields[2].offset = 8;
142  cloud.fields[2].data_type = PointCloudTypes::FLOAT32;
143  cloud.fields[2].count = 4;
144  cloud.fields[3].name = "r";
145  cloud.fields[3].offset = 12;
146  cloud.fields[3].data_type = PointCloudTypes::UINT8;
147  cloud.fields[3].count = 1;
148  cloud.fields[4].name = "g";
149  cloud.fields[4].offset = 13;
150  cloud.fields[4].data_type = PointCloudTypes::UINT8;
151  cloud.fields[4].count = 1;
152  cloud.fields[5].name = "b";
153  cloud.fields[5].offset = 14;
154  cloud.fields[5].data_type = PointCloudTypes::UINT8;
155  cloud.fields[5].count = 1;
156  cloud.is_bigendian = false;
157  cloud.point_step = 16;
158  cloud.width = cloud_raw->width;
159  cloud.height = cloud_raw->height;
160  cloud.data.length(npoint*cloud.point_step);
161  cloud.row_step = cloud.width*cloud.point_step;
162  cloud.is_dense = cloud_raw->is_dense;
163  float *ptr = (float *)cloud.data.get_buffer();
164  std::cout << "npoint = " << npoint << std::endl;
165  for (int i=0; i<npoint; i++){
166  ptr[0] = cloud_raw->points[i].x;
167  ptr[1] = cloud_raw->points[i].y;
168  ptr[2] = cloud_raw->points[i].z;
169  unsigned char *rgb = (unsigned char *)(ptr+3);
170  rgb[0] = cloud_raw->points[i].r;
171  rgb[1] = cloud_raw->points[i].g;
172  rgb[2] = cloud_raw->points[i].b;
173  ptr += 4;
174  }
175 }
176 
177 void PCDLoader::updateOffsetToCloudXYZ(void)
178 {
179  pcl::PointCloud<pcl::PointXYZ>::Ptr clouds(new pcl::PointCloud<pcl::PointXYZ>);
180  for (unsigned int i=0; i<m_offset.length(); i++){
181  const OpenHRP::PCDOffset& offset = m_offset[i];
182  const std::string label(offset.label);
183  if( m_clouds_xyz.find(label) != m_clouds_xyz.end() ){
184  const hrp::Vector3 center(offset.center.x, offset.center.y, offset.center.z);
185  const hrp::Vector3 offsetP(offset.data.position.x, offset.data.position.y, offset.data.position.z);
186  const hrp::Matrix33 offsetR(hrp::rotFromRpy(offset.data.orientation.r,
187  offset.data.orientation.p,
188  offset.data.orientation.y));
189 
190  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_raw = m_clouds_xyz[label];
191  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_new(new pcl::PointCloud<pcl::PointXYZ>);
192  int npoint = cloud_raw->points.size();
193  cloud_new->points.resize(npoint);
194  cloud_new->width = cloud_raw->width;
195  cloud_new->height = cloud_raw->height;
196  cloud_new->is_dense = cloud_raw->is_dense;
197  hrp::Vector3 point, point_new;
198  for(int j = 0 ; j < npoint ; j++){
199  point << cloud_raw->points[j].x, cloud_raw->points[j].y, cloud_raw->points[j].z;
200  point_new = offsetR * (point - center) + center + offsetP;
201  cloud_new->points[j].x = point_new[0];
202  cloud_new->points[j].y = point_new[1];
203  cloud_new->points[j].z = point_new[2];
204  }
205  *clouds += *cloud_new;
206  }
207  }
208  setCloudXYZ(m_cloud, clouds);
209 }
210 
211 void PCDLoader::updateOffsetToCloudXYZRGB(void)
212 {
213  pcl::PointCloud<pcl::PointXYZRGB>::Ptr clouds(new pcl::PointCloud<pcl::PointXYZRGB>);
214  for (unsigned int i=0; i<m_offset.length(); i++){
215  const OpenHRP::PCDOffset& offset = m_offset[i];
216  const std::string label(offset.label);
217  if( m_clouds_xyzrgb.find(label) != m_clouds_xyzrgb.end() ){
218  const hrp::Vector3 center(offset.center.x, offset.center.y, offset.center.z);
219  const hrp::Vector3 offsetP(offset.data.position.x, offset.data.position.y, offset.data.position.z);
220  const hrp::Matrix33 offsetR(hrp::rotFromRpy(offset.data.orientation.r,
221  offset.data.orientation.p,
222  offset.data.orientation.y));
223 
224  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_raw = m_clouds_xyzrgb[label];
225  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_new(new pcl::PointCloud<pcl::PointXYZRGB>);
226  int npoint = cloud_raw->points.size();
227  cloud_new->points.resize(npoint);
228  cloud_new->width = cloud_raw->width;
229  cloud_new->height = cloud_raw->height;
230  cloud_new->is_dense = cloud_raw->is_dense;
231  hrp::Vector3 point, point_new;
232  for(int j = 0 ; j < npoint ; j++){
233  point << cloud_raw->points[j].x, cloud_raw->points[j].y, cloud_raw->points[j].z;
234  point_new = offsetR * (point - center) + center + offsetP;
235  cloud_new->points[j].x = point_new[0];
236  cloud_new->points[j].y = point_new[1];
237  cloud_new->points[j].z = point_new[2];
238  }
239  *clouds += *cloud_new;
240  }
241  }
242  setCloudXYZRGB(m_cloud, clouds);
243 }
244 
245 /*
246 RTC::ReturnCode_t PCDLoader::onFinalize()
247 {
248  return RTC::RTC_OK;
249 }
250 */
251 
252 /*
253 RTC::ReturnCode_t PCDLoader::onStartup(RTC::UniqueId ec_id)
254 {
255  return RTC::RTC_OK;
256 }
257 */
258 
259 /*
260 RTC::ReturnCode_t PCDLoader::onShutdown(RTC::UniqueId ec_id)
261 {
262  return RTC::RTC_OK;
263 }
264 */
265 
266 RTC::ReturnCode_t PCDLoader::onActivated(RTC::UniqueId ec_id)
267 {
268  std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl;
269  return RTC::RTC_OK;
270 }
271 
272 RTC::ReturnCode_t PCDLoader::onDeactivated(RTC::UniqueId ec_id)
273 {
274  std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl;
275  return RTC::RTC_OK;
276 }
277 
278 RTC::ReturnCode_t PCDLoader::onExecute(RTC::UniqueId ec_id)
279 {
280  //std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl;
281  if ( !m_path.empty()){
282  pcl::PCDReader reader;
283 
284  if (m_fields=="XYZ"){
285  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
286  if (reader.read (m_path, *cloud)){
287  std::cerr << m_profile.instance_name << ": failed to load("
288  << m_path << ")" << std::endl;
289  m_path = "";
290  return RTC::RTC_OK;
291  }
292  setCloudXYZ(m_cloud, cloud);
293  }else if(m_fields=="XYZRGB"){
294  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
295  if (reader.read (m_path, *cloud)){
296  std::cerr << m_profile.instance_name << ": failed to load("
297  << m_path << ")" << std::endl;
298  m_path = "";
299  return RTC::RTC_OK;
300  }
301  setCloudXYZRGB(m_cloud, cloud);
302  }else{
303  std::cerr << "fields[" << m_fields << "] is not supported" << std::endl;
304  }
305  m_cloudOut.write();
306  m_isOutputOut.write();
307  m_path = "";
308  }
309 
310  if( m_offsetIn.isNew() ){
311  m_offsetIn.read();
312  if( !m_clouds_xyz.empty() ){
313  updateOffsetToCloudXYZ();
314  m_cloudOut.write();
315  m_isOutputOut.write();
316  }
317  else if( !m_clouds_xyzrgb.empty() ){
318  updateOffsetToCloudXYZRGB();
319  m_cloudOut.write();
320  m_isOutputOut.write();
321  }
322  }
323 
324  return RTC::RTC_OK;
325 }
326 
327 /*
328 RTC::ReturnCode_t PCDLoader::onAborting(RTC::UniqueId ec_id)
329 {
330  return RTC::RTC_OK;
331 }
332 */
333 
334 /*
335 RTC::ReturnCode_t PCDLoader::onError(RTC::UniqueId ec_id)
336 {
337  return RTC::RTC_OK;
338 }
339 */
340 
341 /*
342 RTC::ReturnCode_t PCDLoader::onReset(RTC::UniqueId ec_id)
343 {
344  return RTC::RTC_OK;
345 }
346 */
347 
348 /*
349 RTC::ReturnCode_t PCDLoader::onStateUpdate(RTC::UniqueId ec_id)
350 {
351  return RTC::RTC_OK;
352 }
353 */
354 
355 /*
356 RTC::ReturnCode_t PCDLoader::onRateChanged(RTC::UniqueId ec_id)
357 {
358  return RTC::RTC_OK;
359 }
360 */
361 
362 
363 bool PCDLoader::load(const std::string& filename, const std::string& label)
364 {
365  pcl::PCDReader reader;
366  if( m_fields == "XYZ" ){
367  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
368  if (reader.read (filename, *cloud)){
369  std::cerr << m_profile.instance_name << ": failed to load("
370  << filename << ")" << std::endl;
371  return RTC::RTC_OK;
372  }
373  else
374  std::cout << "Loading " << filename << " with XYZ fields." << std::endl;
375  m_clouds_xyz[label] = cloud;
376  }
377  else if( m_fields == "XYZRGB" ){
378  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
379  if (reader.read (filename, *cloud)){
380  std::cerr << m_profile.instance_name << ": failed to load("
381  << filename << ")" << std::endl;
382  return RTC::RTC_OK;
383  }
384  else
385  std::cout << "Loading " << filename << " with XYZRGB fields." << std::endl;
386  m_clouds_xyzrgb[label] = cloud;
387  }else{
388  std::cerr << "fields[" << m_fields << "] is not supported" << std::endl;
389  }
390 }
391 
392 extern "C"
393 {
394 
395  void PCDLoaderInit(RTC::Manager* manager)
396  {
397  RTC::Properties profile(spec);
398  manager->registerFactory(profile,
399  RTC::Create<PCDLoader>,
400  RTC::Delete<PCDLoader>);
401  }
402 
403 };
404 
405 
RTC::RTObject_impl::m_profile
ComponentProfile m_profile
profile
png_infop png_charpp int png_charpp profile
clouds
std::vector< pcl::PointCloud< pcl::PointXYZRGB >::Ptr > clouds
Definition: PointCloudLogViewer.cpp:8
PCDLoader::m_path
std::string m_path
Definition: PCDLoader.h:156
PCDLoader::m_clouds_xyz
boost::unordered_map< std::string, pcl::PointCloud< pcl::PointXYZ >::Ptr > m_clouds_xyz
Definition: PCDLoader.h:158
reader
void * reader(void *arg)
PCDLoader::setCloudXYZRGB
void setCloudXYZRGB(PointCloudTypes::PointCloud &cloud, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr cloud_raw)
Definition: PCDLoader.cpp:126
ptr
png_voidp ptr
PCDLoader::m_fields
std::string m_fields
Definition: PCDLoader.h:156
PCDLoader::onDeactivated
virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id)
Definition: PCDLoader.cpp:272
PCDLoader::updateOffsetToCloudXYZRGB
void updateOffsetToCloudXYZRGB(void)
Definition: PCDLoader.cpp:211
spec
static const char * spec[]
Definition: PCDLoader.cpp:17
PCDLoaderInit
void PCDLoaderInit(RTC::Manager *manager)
Definition: PCDLoader.cpp:395
PCDLoader::setCloudXYZ
void setCloudXYZ(PointCloudTypes::PointCloud &cloud, const pcl::PointCloud< pcl::PointXYZ >::Ptr cloud_raw)
Definition: PCDLoader.cpp:91
PCDLoader::onInitialize
virtual RTC::ReturnCode_t onInitialize()
Definition: PCDLoader.cpp:55
i
png_uint_32 i
RTC::RTObject_impl::getProperties
coil::Properties & getProperties()
PCDLoader::m_clouds_xyzrgb
boost::unordered_map< std::string, pcl::PointCloud< pcl::PointXYZRGB >::Ptr > m_clouds_xyzrgb
Definition: PCDLoader.h:159
RTC::RTObject_impl::addOutPort
bool addOutPort(const char *name, OutPortBase &outport)
PCDLoader::PCDLoader
PCDLoader(RTC::Manager *manager)
Constructor.
Definition: PCDLoader.cpp:37
hrp::Vector3
Eigen::Vector3d Vector3
PCDLoader::m_service0
PCDLoaderService_impl m_service0
Definition: PCDLoader.h:138
hrp::rotFromRpy
Matrix33 rotFromRpy(const Vector3 &rpy)
hrp::Matrix33
Eigen::Matrix3d Matrix33
PCDLoader.h
PCD file loader.
PCDLoader::m_offsetIn
InPort< OpenHRP::PCDOffsetSeq > m_offsetIn
Definition: PCDLoader.h:119
PCDLoader::onActivated
virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id)
Definition: PCDLoader.cpp:266
RTC::UniqueId
ExecutionContextHandle_t UniqueId
RTC::RTObject_impl::bindParameter
bool bindParameter(const char *param_name, VarType &var, const char *def_val, bool(*trans)(VarType &, const char *)=coil::stringTo)
PCDLoader::m_cloudOut
OutPort< PointCloudTypes::PointCloud > m_cloudOut
Definition: PCDLoader.h:125
j
def j(str, encoding="cp932")
PCDLoader::m_isOutputOut
OutPort< RTC::TimedBoolean > m_isOutputOut
Definition: PCDLoader.h:126
PCDLoader::onExecute
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id)
Definition: PCDLoader.cpp:278
prop
prop
PCDLoaderService_impl::setComp
void setComp(PCDLoader *i_comp)
Definition: PCDLoaderService_impl.cpp:13
PCDLoader::m_offset
OpenHRP::PCDOffsetSeq m_offset
Definition: PCDLoader.h:113
PCDLoader::updateOffsetToCloudXYZ
void updateOffsetToCloudXYZ(void)
Definition: PCDLoader.cpp:177
PCDLoader::m_PCDLoaderServicePort
RTC::CorbaPort m_PCDLoaderServicePort
Definition: PCDLoader.h:132
RTC::InPort::isNew
virtual bool isNew()
RTC::RTObject_impl::addPort
bool addPort(PortBase &port)
RTC::OutPort::write
virtual bool write(DataType &value)
PCDLoader::m_cloud
PointCloudTypes::PointCloud m_cloud
Definition: PCDLoader.h:112
PCDLoader::m_isOutput
RTC::TimedBoolean m_isOutput
Definition: PCDLoader.h:114
RTC::RTObject_impl::addInPort
bool addInPort(const char *name, InPortBase &inport)
RTC::Manager::registerFactory
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
PCDLoader::load
bool load(const std::string &filename, const std::string &label)
Definition: PCDLoader.cpp:363
PCDLoader::~PCDLoader
virtual ~PCDLoader()
Destructor.
Definition: PCDLoader.cpp:49
RTC::CorbaPort::registerProvider
bool registerProvider(const char *instance_name, const char *type_name, PortableServer::RefCountServantBase &provider)

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