PointCloudViewer.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 "PointCloudViewer.h"
14 #include "hrpsys/idl/pointcloud.hh"
15 #include <string>
16 
17 // Module specification
18 // <rtc-template block="module_spec">
19 static const char* spec[] =
20  {
21  "implementation_id", "PointCloudViewer",
22  "type_name", "PointCloudViewer",
23  "description", "Point Cloud Viewer",
24  "version", HRPSYS_PACKAGE_VERSION,
25  "vendor", "AIST",
26  "category", "example",
27  "activity_type", "DataFlowComponent",
28  "max_instance", "10",
29  "language", "C++",
30  "lang_type", "compile",
31  // Configuration variables
32 
33  ""
34  };
35 // </rtc-template>
36 
38  : RTC::DataFlowComponentBase(manager),
39  // <rtc-template block="initializer">
40  m_cloudIn("cloud", m_cloud),
41  // </rtc-template>
42  m_viewer("Point Cloud Viewer"),
43  dummy(0)
44 {
45 }
46 
48 {
49 }
50 
51 
52 
53 RTC::ReturnCode_t PointCloudViewer::onInitialize()
54 {
55  //std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
56  // <rtc-template block="bind_config">
57  // Bind variables and configuration variable
58 
59  // </rtc-template>
60 
61  // Registration: InPort/OutPort/Service
62  // <rtc-template block="registration">
63  // Set InPort buffers
64  addInPort("cloudIn", m_cloudIn);
65 
66  // Set OutPort buffer
67 
68  // Set service provider to Ports
69 
70  // Set service consumers to Ports
71 
72  // Set CORBA Service Ports
73 
74  // </rtc-template>
75 
77 
78  return RTC::RTC_OK;
79 }
80 
81 
82 
83 /*
84 RTC::ReturnCode_t PointCloudViewer::onFinalize()
85 {
86  return RTC::RTC_OK;
87 }
88 */
89 
90 /*
91 RTC::ReturnCode_t PointCloudViewer::onStartup(RTC::UniqueId ec_id)
92 {
93  return RTC::RTC_OK;
94 }
95 */
96 
97 /*
98 RTC::ReturnCode_t PointCloudViewer::onShutdown(RTC::UniqueId ec_id)
99 {
100  return RTC::RTC_OK;
101 }
102 */
103 
105 {
106  std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl;
107  return RTC::RTC_OK;
108 }
109 
111 {
112  std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl;
113  return RTC::RTC_OK;
114 }
115 
117 {
118  //std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl;
119 
120  if (m_cloudIn.isNew()){
121  m_cloudIn.read();
122 
123  bool is_color_points = false;
124  for (int i = 0; i < m_cloud.fields.length(); i++) {
125  std::string tmp_name(m_cloud.fields[i].name);
126  if (tmp_name.find("r") != std::string::npos || tmp_name.find("g") != std::string::npos || tmp_name.find("b") != std::string::npos) {
127  is_color_points = true; // color pointcloud should have rgb field
128  }
129  }
130 
131  // currently only support PointXYZ and PointXYZRGB
132  if (is_color_points) {
133  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
134  cloud->is_dense = m_cloud.is_dense; // need to handle pointcloud which has invalid points (is_dense = false)
135  cloud->points.resize(m_cloud.width*m_cloud.height);
136  float *src = reinterpret_cast<float*>(m_cloud.data.get_buffer());
137  for (unsigned int i = 0; i< cloud->points.size(); i++) {
138  cloud->points[i].x = src[0];
139  cloud->points[i].y = src[1];
140  cloud->points[i].z = src[2];
141  unsigned char *rgb = (unsigned char *)(src+3);
142  cloud->points[i].r = rgb[0];
143  cloud->points[i].g = rgb[1];
144  cloud->points[i].b = rgb[2];
145  src += 4;
146  }
147  if (!m_viewer.wasStopped()){
148  m_viewer.showCloud(cloud);
149  }
150  } else {
151  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
152  cloud->points.resize(m_cloud.width*m_cloud.height);
153  float *src = (float *)m_cloud.data.get_buffer();
154  for (unsigned int i=0; i<cloud->points.size(); i++){
155  cloud->points[i].x = src[0];
156  cloud->points[i].y = src[1];
157  cloud->points[i].z = src[2];
158  src += 4;
159  }
160  if (!m_viewer.wasStopped()){
161  m_viewer.showCloud(cloud);
162  }
163  }
164  }
165 
166  return RTC::RTC_OK;
167 }
168 
169 /*
170 RTC::ReturnCode_t PointCloudViewer::onAborting(RTC::UniqueId ec_id)
171 {
172  return RTC::RTC_OK;
173 }
174 */
175 
176 /*
177 RTC::ReturnCode_t PointCloudViewer::onError(RTC::UniqueId ec_id)
178 {
179  return RTC::RTC_OK;
180 }
181 */
182 
183 /*
184 RTC::ReturnCode_t PointCloudViewer::onReset(RTC::UniqueId ec_id)
185 {
186  return RTC::RTC_OK;
187 }
188 */
189 
190 /*
191 RTC::ReturnCode_t PointCloudViewer::onStateUpdate(RTC::UniqueId ec_id)
192 {
193  return RTC::RTC_OK;
194 }
195 */
196 
197 /*
198 RTC::ReturnCode_t PointCloudViewer::onRateChanged(RTC::UniqueId ec_id)
199 {
200  return RTC::RTC_OK;
201 }
202 */
203 
204 
205 
206 extern "C"
207 {
208 
210  {
212  manager->registerFactory(profile,
213  RTC::Create<PointCloudViewer>,
214  RTC::Delete<PointCloudViewer>);
215  }
216 
217 };
218 
219 
ComponentProfile m_profile
virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id)
png_infop png_charpp int png_charpp profile
pcl::visualization::CloudViewer m_viewer
void PointCloudViewerInit(RTC::Manager *manager)
virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id)
png_uint_32 i
coil::Properties & getProperties()
virtual RTC::ReturnCode_t onInitialize()
ExecutionContextHandle_t UniqueId
PointCloudViewer(RTC::Manager *manager)
Constructor.
InPort< PointCloudTypes::PointCloud > m_cloudIn
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id)
prop
Point Cloud Viewer.
static const char * spec[]
virtual bool isNew()
bool addInPort(const char *name, InPortBase &inport)
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
PointCloudTypes::PointCloud m_cloud
virtual ~PointCloudViewer()
Destructor.


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