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