1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2014-, Open Perception, Inc.
6 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of the copyright holder(s) nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 * Author: Victor Lamoine (victor.lamoine@gmail.com)
37 */
38
39 #include <pcl/pcl_config.h>
40 #include <pcl/io/davidsdk_grabber.h>
41 #include <pcl/exceptions.h>
42 #include <pcl/common/io.h>
43 #include <pcl/conversions.h>
44 #include <pcl/io/ply_io.h>
45 #include <pcl/io/vtk_lib_io.h>
46 #include <pcl/console/print.h>
47 #include <pcl/point_types.h>
48
49 // Possible improvements:
50 // TODO: Add presets for david::CodedLightPhaseShiftParams to enable easy scan quality changing
51 // TODO: Add texture support (call .Scan () instead of .Scan (false) and properly convert data
52 // TODO: Use mesh IDs rather than clearing all meshes every time
53 // TODO: In processGrabbing, start scanning again while transferring the mesh (not possible with SDK 1.5.2 because ExportMesh() is blocking)
54
55 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
DavidSDKGrabber()56 pcl::DavidSDKGrabber::DavidSDKGrabber () :
57 client_connected_ (false),
58 running_ (false),
59 local_path_ ("C:/temp"),
60 remote_path_ ("C:/temp"),
61 file_format_ ("stl")
62 {
63 point_cloud_signal_ = createSignal<sig_cb_davidsdk_point_cloud> ();
64 mesh_signal_ = createSignal<sig_cb_davidsdk_mesh> ();
65 image_signal_ = createSignal<sig_cb_davidsdk_image> ();
66 point_cloud_image_signal_ = createSignal<sig_cb_davidsdk_point_cloud_image> ();
67 mesh_image_signal_ = createSignal<sig_cb_davidsdk_mesh_image> ();
68 }
69
70 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
~DavidSDKGrabber()71 pcl::DavidSDKGrabber::~DavidSDKGrabber () noexcept
72 {
73 try
74 {
75 stop ();
76
77 disconnect_all_slots<sig_cb_davidsdk_point_cloud> ();
78 disconnect_all_slots<sig_cb_davidsdk_mesh> ();
79 disconnect_all_slots<sig_cb_davidsdk_image> ();
80 disconnect_all_slots<sig_cb_davidsdk_point_cloud_image> ();
81 disconnect_all_slots<sig_cb_davidsdk_mesh_image> ();
82 }
83 catch (...)
84 {
85 // destructor never throws
86 }
87 }
88
89 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
90 david::ServerInfo
connect(const std::string & address,std::uint16_t port)91 pcl::DavidSDKGrabber::connect (const std::string &address,
92 std::uint16_t port)
93 {
94 david::ServerInfo server_info;
95
96 if (client_connected_)
97 return (server_info);
98
99 try
100 {
101 david_.Connect (address, port);
102 client_connected_ = true;
103 }
104 catch (david::Exception& e)
105 {
106 e.PrintError ();
107 }
108
109 return (server_info);
110 }
111
112 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
113 void
disconnect(const bool stop_server)114 pcl::DavidSDKGrabber::disconnect (const bool stop_server)
115 {
116 if (!client_connected_)
117 return;
118
119 try
120 {
121 david_.Disconnect (stop_server);
122 }
123 catch (david::Exception& e)
124 {
125 e.PrintError ();
126 }
127
128 client_connected_ = false;
129 }
130
131 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
132 void
start()133 pcl::DavidSDKGrabber::start ()
134 {
135 if (isRunning ())
136 return;
137
138 frequency_.reset ();
139 running_ = true;
140 grabber_thread_ = std::thread (&pcl::DavidSDKGrabber::processGrabbing, this);
141 }
142
143 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
144 void
stop()145 pcl::DavidSDKGrabber::stop ()
146 {
147 if (running_)
148 {
149 running_ = false; // Stop processGrabbing () callback
150
151 grabber_thread_.join (); // join () waits for the thread to finish it's last iteration
152 // See: http://www.boost.org/doc/libs/1_54_0/doc/html/thread/thread_management.html#thread.thread_management.thread.join
153 }
154 }
155
156 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
157 bool
isRunning() const158 pcl::DavidSDKGrabber::isRunning () const
159 {
160 return (running_);
161 }
162
163 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
164 bool
isConnected() const165 pcl::DavidSDKGrabber::isConnected () const
166 {
167 return (client_connected_);
168 }
169
170 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
171 std::string
getName() const172 pcl::DavidSDKGrabber::getName () const
173 {
174 return ("DavidSDKGrabber");
175 }
176
177 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
178 std::string
getLocalPath()179 pcl::DavidSDKGrabber::getLocalPath ()
180 {
181 return (local_path_);
182 }
183
184 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
185 std::string
getRemotePath()186 pcl::DavidSDKGrabber::getRemotePath ()
187 {
188 return (remote_path_);
189 }
190
191 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
192 void
setFileFormatToOBJ()193 pcl::DavidSDKGrabber::setFileFormatToOBJ ()
194 {
195 file_format_ = "obj";
196 }
197
198 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
199 void
setFileFormatToPLY()200 pcl::DavidSDKGrabber::setFileFormatToPLY ()
201 {
202 file_format_ = "ply";
203 }
204
205 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
206 void
setFileFormatToSTL()207 pcl::DavidSDKGrabber::setFileFormatToSTL ()
208 {
209 file_format_ = "stl";
210 }
211
212 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
213 std::string
getFileFormat()214 pcl::DavidSDKGrabber::getFileFormat ()
215 {
216 return (file_format_);
217 }
218
219 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
220 void
setLocalPath(std::string path)221 pcl::DavidSDKGrabber::setLocalPath (std::string path)
222 {
223 local_path_ = path;
224
225 if (path.empty ())
226 local_path_ = "C:/temp";
227 }
228
229 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
230 void
setRemotePath(std::string path)231 pcl::DavidSDKGrabber::setRemotePath (std::string path)
232 {
233 remote_path_ = path;
234
235 if (path.empty ())
236 remote_path_ = local_path_;
237 }
238
239 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
240 void
setLocalAndRemotePaths(std::string local_path,std::string remote_path)241 pcl::DavidSDKGrabber::setLocalAndRemotePaths (std::string local_path,
242 std::string remote_path)
243 {
244 setLocalPath (local_path);
245 setRemotePath (remote_path);
246 }
247
248 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
249 bool
calibrate(double grid_size)250 pcl::DavidSDKGrabber::calibrate (double grid_size)
251 {
252 if (!client_connected_ || running_)
253 return (false);
254
255 try
256 {
257 david_.sls ().Calibrate (grid_size);
258 }
259 catch (david::Exception& e)
260 {
261 e.PrintError ();
262 return (false);
263 }
264 return (true);
265 }
266
267 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
268 bool
grabSingleCloud(pcl::PointCloud<pcl::PointXYZ> & cloud)269 pcl::DavidSDKGrabber::grabSingleCloud (pcl::PointCloud<pcl::PointXYZ> &cloud)
270 {
271 if (!client_connected_ || running_)
272 return (false);
273
274 try
275 {
276 david_.sls ().Scan (false);
277 david_.fusion ().DeleteAllMeshes ();
278 david_.sls ().AddScanToShapeFusion ();
279 david_.sls ().ExportMesh (remote_path_ + "scan." + file_format_);
280
281 pcl::PolygonMesh mesh;
282 if (file_format_ == "obj")
283 {
284 if (pcl::io::loadPolygonFileOBJ (local_path_ + "scan." + file_format_, mesh) == 0)
285 return (false);
286 }
287 else if (file_format_ == "ply")
288 {
289 if (pcl::io::loadPolygonFilePLY (local_path_ + "scan." + file_format_, mesh) == 0)
290 return (false);
291 }
292 else if (file_format_ == "stl")
293 {
294 if (pcl::io::loadPolygonFileSTL (local_path_ + "scan." + file_format_, mesh) == 0)
295 return (false);
296 }
297 else
298 return (false);
299
300 pcl::fromPCLPointCloud2 (mesh.cloud, cloud);
301 }
302 catch (david::Exception& e)
303 {
304 e.PrintError ();
305 return (false);
306 }
307 return (true);
308 }
309
310 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
311 bool
grabSingleMesh(pcl::PolygonMesh & mesh)312 pcl::DavidSDKGrabber::grabSingleMesh (pcl::PolygonMesh &mesh)
313 {
314 if (!client_connected_ || running_)
315 return (false);
316
317 try
318 {
319 david_.sls ().Scan (false);
320 david_.fusion ().DeleteAllMeshes ();
321 david_.sls ().AddScanToShapeFusion ();
322 david_.sls ().ExportMesh (remote_path_ + "scan." + file_format_);
323
324 if (file_format_ == "obj")
325 {
326 if (pcl::io::loadPolygonFileOBJ (local_path_ + "scan." + file_format_, mesh) == 0)
327 return (false);
328 }
329 else if (file_format_ == "ply")
330 {
331 if (pcl::io::loadPolygonFilePLY (local_path_ + "scan." + file_format_, mesh) == 0)
332 return (false);
333 }
334 else if (file_format_ == "stl")
335 {
336 if (pcl::io::loadPolygonFileSTL (local_path_ + "scan." + file_format_, mesh) == 0)
337 return (false);
338 }
339 else
340 return (false);
341
342 }
343 catch (david::Exception& e)
344 {
345 e.PrintError ();
346 return (false);
347 }
348 return (true);
349 }
350
351 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
352 float
getFramesPerSecond() const353 pcl::DavidSDKGrabber::getFramesPerSecond () const
354 {
355 std::lock_guard<std::mutex> lock (fps_mutex_);
356 return (frequency_.getFrequency ());
357 }
358
359 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
360 void
processGrabbing()361 pcl::DavidSDKGrabber::processGrabbing ()
362 {
363 bool continue_grabbing = running_;
364 while (continue_grabbing)
365 {
366 try
367 {
368 // Publish cloud / images
369 if (num_slots<sig_cb_davidsdk_point_cloud> () > 0 || num_slots<sig_cb_davidsdk_mesh> () > 0 || num_slots<sig_cb_davidsdk_image> () > 0
370 || num_slots<sig_cb_davidsdk_point_cloud_image> () > 0 || num_slots<sig_cb_davidsdk_mesh_image> () > 0)
371 {
372 pcl::PolygonMesh::Ptr mesh;
373 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
374 pcl::PCLImage::Ptr image;
375
376 fps_mutex_.lock ();
377 frequency_.event ();
378 fps_mutex_.unlock ();
379
380 // We need the image
381 if (num_slots<sig_cb_davidsdk_image> () > 0 || num_slots<sig_cb_davidsdk_point_cloud_image> () > 0 || num_slots<sig_cb_davidsdk_mesh_image> () > 0)
382 {
383 image.reset (new pcl::PCLImage);
384 int width, height;
385 david_.sls ().GetLiveImage (image->data, width, height);
386 image->width = (std::uint32_t) width;
387 image->height = (std::uint32_t) height;
388 image->encoding = "CV_8UC1";
389 }
390
391 // We need the cloud or mesh
392 if (num_slots<sig_cb_davidsdk_point_cloud> () > 0 || num_slots<sig_cb_davidsdk_mesh> () > 0 || num_slots<sig_cb_davidsdk_point_cloud_image> () > 0
393 || num_slots<sig_cb_davidsdk_mesh_image> () > 0)
394 {
395 mesh.reset (new pcl::PolygonMesh);
396 david_.sls ().Scan (false);
397 david_.fusion ().DeleteAllMeshes ();
398 david_.sls ().AddScanToShapeFusion ();
399 david_.sls ().ExportMesh (remote_path_ + "scan." + file_format_);
400
401 if (file_format_ == "obj")
402 {
403 if (pcl::io::loadPolygonFileOBJ (local_path_ + "scan." + file_format_, *mesh) == 0)
404 return;
405 }
406 else if (file_format_ == "ply")
407 {
408 if (pcl::io::loadPolygonFilePLY (local_path_ + "scan." + file_format_, *mesh) == 0)
409 return;
410 }
411 else if (file_format_ == "stl")
412 {
413 if (pcl::io::loadPolygonFileSTL (local_path_ + "scan." + file_format_, *mesh) == 0)
414 return;
415 }
416 else
417 return;
418
419 if (num_slots<sig_cb_davidsdk_point_cloud> () > 0 || num_slots<sig_cb_davidsdk_point_cloud_image> () > 0)
420 {
421 cloud.reset (new PointCloud<pcl::PointXYZ>);
422 pcl::fromPCLPointCloud2 (mesh->cloud, *cloud);
423 }
424 }
425
426 // Publish signals
427 if (num_slots<sig_cb_davidsdk_point_cloud_image> () > 0)
428 point_cloud_image_signal_->operator () (cloud, image);
429 if (num_slots<sig_cb_davidsdk_mesh_image> () > 0)
430 mesh_image_signal_->operator () (mesh, image);
431 else if (num_slots<sig_cb_davidsdk_point_cloud> () > 0)
432 point_cloud_signal_->operator () (cloud);
433 else if (num_slots<sig_cb_davidsdk_mesh> () > 0)
434 mesh_signal_->operator () (mesh);
435 else if (num_slots<sig_cb_davidsdk_image> () > 0)
436 image_signal_->operator () (image);
437 }
438 continue_grabbing = running_;
439 }
440 catch (david::Exception& e)
441 {
442 e.PrintError ();
443 }
444 }
445 }
446