Point Cloud Library (PCL)  1.14.1-dev
ensenso_grabber.h
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 #pragma once
40 
41 #include <pcl/pcl_config.h>
42 
43 #include <pcl/common/time.h>
44 #include <pcl/common/io.h>
45 #include <Eigen/Geometry>
46 #include <Eigen/StdVector>
47 
48 #include <pcl/io/grabber.h>
49 #include <pcl/common/synchronizer.h>
50 
51 #include <nxLib.h> // Ensenso SDK
52 
53 #include <thread>
54 
55 namespace pcl
56 {
57  struct PointXYZ;
58  template <typename T> class PointCloud;
59 
60  /** @brief Grabber for IDS-Imaging Ensenso's devices.\n
61  * The [Ensenso SDK](http://www.ensenso.de/manual/) allow to use multiple Ensenso devices to produce a single cloud.\n
62  * This feature is not implemented here, it is up to the user to configure multiple Ensenso cameras.\n
63  * @author Victor Lamoine (victor.lamoine@gmail.com)\n
64  * @ingroup io
65  */
67  {
68  using PairOfImages = std::pair<pcl::PCLImage, pcl::PCLImage>;
69 
70  public:
71  /** @cond */
72  using Ptr = shared_ptr<EnsensoGrabber>;
73  using ConstPtr = shared_ptr<const EnsensoGrabber>;
74 
75  // Define callback signature typedefs
76  using sig_cb_ensenso_point_cloud = void(const pcl::PointCloud<pcl::PointXYZ>::Ptr&);
77 
78  using sig_cb_ensenso_images = void(const shared_ptr<PairOfImages>&);
79 
80  using sig_cb_ensenso_point_cloud_images = void(const pcl::PointCloud<pcl::PointXYZ>::Ptr&,const shared_ptr<PairOfImages>&);
81 
82  /** @endcond */
83 
84  /** @brief Constructor */
86 
87  /** @brief Destructor inherited from the Grabber interface. It never throws. */
88 
89  ~EnsensoGrabber () noexcept override;
90 
91  /** @brief Searches for available devices
92  * @returns The number of Ensenso devices connected */
93  int
94  enumDevices () const;
95 
96  /** @brief Opens an Ensenso device
97  * @param[in] device The device ID to open
98  * @return True if successful, false otherwise */
99  bool
100  openDevice (const int device = 0);
101 
102  /** @brief Closes the Ensenso device
103  * @return True if successful, false otherwise */
104  bool
105  closeDevice ();
106 
107  /** @brief Start the point cloud and or image acquisition
108  * @note Opens device "0" if no device is open */
109  void
110  start () override;
111 
112  /** @brief Stop the data acquisition */
113  void
114  stop () override;
115 
116  /** @brief Check if the data acquisition is still running
117  * @return True if running, false otherwise */
118  bool
119  isRunning () const override;
120 
121  /** @brief Check if a TCP port is opened
122  * @return True if open, false otherwise */
123  bool
124  isTcpPortOpen () const;
125 
126  /** @brief Get class name
127  * @returns A string containing the class name */
128  std::string
129  getName () const override;
130 
131  /** @brief Configure Ensenso capture settings
132  * @param[in] auto_exposure If set to yes, the exposure parameter will be ignored
133  * @param[in] auto_gain If set to yes, the gain parameter will be ignored
134  * @param[in] bining Pixel bining: 1, 2 or 4
135  * @param[in] exposure In milliseconds, from 0.01 to 20 ms
136  * @param[in] front_light Infrared front light (useful for calibration)
137  * @param[in] gain Float between 1 and 4
138  * @param[in] gain_boost
139  * @param[in] hardware_gamma
140  * @param[in] hdr High Dynamic Range (check compatibility with other options in Ensenso manual)
141  * @param[in] pixel_clock In MegaHertz, from 5 to 85
142  * @param[in] projector Use the central infrared projector or not
143  * @param[in] target_brightness Between 40 and 210
144  * @param[in] trigger_mode
145  * @param[in] use_disparity_map_area_of_interest
146  * @return True if successful, false otherwise
147  * @note See [Capture tree item](http://www.ensenso.de/manual/index.html?capture.htm) for more
148  * details about the parameters. */
149  bool
150  configureCapture (const bool auto_exposure = true,
151  const bool auto_gain = true,
152  const int bining = 1,
153  const float exposure = 0.32,
154  const bool front_light = false,
155  const int gain = 1,
156  const bool gain_boost = false,
157  const bool hardware_gamma = false,
158  const bool hdr = false,
159  const int pixel_clock = 10,
160  const bool projector = true,
161  const int target_brightness = 80,
162  const std::string trigger_mode = "Software",
163  const bool use_disparity_map_area_of_interest = false) const;
164 
165  /** @brief Capture a single point cloud and store it
166  * @param[out] cloud The cloud to be filled
167  * @return True if successful, false otherwise
168  * @warning A device must be opened and not running */
169  bool
170  grabSingleCloud (pcl::PointCloud<pcl::PointXYZ> &cloud) const;
171 
172  /** @brief Set up the Ensenso sensor and API to do 3D extrinsic calibration using the Ensenso 2D patterns
173  * @param[in] grid_spacing
174  * @return True if successful, false otherwise
175  *
176  * Configures the capture parameters to default values (eg: @c projector = @c false and @c front_light = @c true)
177  * Discards all previous patterns, configures @c grid_spacing
178  * @warning A device must be opened and must not be running.
179  * @note See the [Ensenso manual](http://www.ensenso.de/manual/index.html?calibratehandeyeparameters.htm) for more
180  * information about the extrinsic calibration process.
181  * @note [GridSize](http://www.ensenso.de/manual/index.html?gridsize.htm) item is protected in the NxTree, you can't modify it.
182  */
183  bool
184  initExtrinsicCalibration (const int grid_spacing) const;
185 
186  /** @brief Clear calibration patterns buffer */
187  bool
188  clearCalibrationPatternBuffer () const;
189 
190  /** @brief Captures a calibration pattern
191  * @return the number of calibration patterns stored in the nxTree, -1 on error
192  * @warning A device must be opened and must not be running.
193  * @note You should use @ref initExtrinsicCalibration before */
194  int
195  captureCalibrationPattern () const;
196 
197  /** @brief Estimate the calibration pattern pose
198  * @param[out] pattern_pose the calibration pattern pose
199  * @return true if successful, false otherwise
200  * @warning A device must be opened and must not be running.
201  * @note At least one calibration pattern must have been captured before, use @ref captureCalibrationPattern before */
202  bool
203  estimateCalibrationPatternPose (Eigen::Affine3d &pattern_pose) const;
204 
205  /** @brief Computes the calibration matrix using the collected patterns and the robot poses vector
206  * @param[in] robot_poses A list of robot poses, 1 for each pattern acquired (in the same order)
207  * @param[out] json The extrinsic calibration data in JSON format
208  * @param[in] setup Moving or Fixed, please refer to the Ensenso documentation
209  * @param[in] target Please refer to the Ensenso documentation
210  * @param[in] guess_tf Guess transformation for the calibration matrix (translation in meters)
211  * @param[in] pretty_format JSON formatting style
212  * @return True if successful, false otherwise
213  * @warning This can take up to 120 seconds
214  * @note Check the result with @ref getResultAsJson.
215  * If you want to permanently store the result, use @ref storeEEPROMExtrinsicCalibration. */
216  bool
217  computeCalibrationMatrix (const std::vector<Eigen::Affine3d, Eigen::aligned_allocator<Eigen::Affine3d> > &robot_poses,
218  std::string &json,
219  const std::string setup = "Moving", // Default values: Moving or Fixed
220  const std::string target = "Hand", // Default values: Hand or Workspace
221  const Eigen::Affine3d &guess_tf = Eigen::Affine3d::Identity (),
222  const bool pretty_format = true) const;
223 
224  /** @brief Copy the link defined in the Link node of the nxTree to the EEPROM
225  * @return True if successful, false otherwise
226  * Refer to @ref setExtrinsicCalibration for more information about how the EEPROM works.\n
227  * After calling @ref computeCalibrationMatrix, this enables to permanently store the matrix.
228  * @note The target must be specified (@ref computeCalibrationMatrix specifies the target) */
229  bool
230  storeEEPROMExtrinsicCalibration () const;
231 
232  /** @brief Clear the extrinsic calibration stored in the EEPROM by writing an identity matrix
233  * @return True if successful, false otherwise */
234  bool
235  clearEEPROMExtrinsicCalibration ();
236 
237  /** @brief Update Link node in NxLib tree
238  * @param[in] target "Hand" or "Workspace" for example
239  * @param[in] euler_angle
240  * @param[in] rotation_axis
241  * @param[in] translation Translation in meters
242  * @return True if successful, false otherwise
243  * @warning Translation are in meters, rotation angles in radians! (stored in mm/radians in Ensenso tree)
244  * @note If a calibration has been stored in the EEPROM, it is copied in the Link node at nxLib tree start.
245  * This method overwrites the Link node but does not write to the EEPROM.
246  *
247  * More information on the parameters can be found in [Link node](http://www.ensenso.de/manual/index.html?cameralink.htm)
248  * section of the Ensenso manual.
249  *
250  * The point cloud you get from the Ensenso is already transformed using this calibration matrix.
251  * Make sure it is the identity transformation if you want the original point cloud! (use @ref clearEEPROMExtrinsicCalibration)
252  * Use @ref storeEEPROMExtrinsicCalibration to permanently store this transformation */
253  bool
254  setExtrinsicCalibration (const double euler_angle,
255  Eigen::Vector3d &rotation_axis,
256  const Eigen::Vector3d &translation,
257  const std::string target = "Hand") const;
258 
259  /** @brief Update Link node in NxLib tree with an identity matrix
260  * @param[in] target "Hand" or "Workspace" for example
261  * @return True if successful, false otherwise */
262  bool
263  setExtrinsicCalibration (const std::string target = "Hand");
264 
265  /** @brief Update Link node in NxLib tree
266  * @param[in] transformation Transformation matrix
267  * @param[in] target "Hand" or "Workspace" for example
268  * @return True if successful, false otherwise
269  * @warning Translation are in meters, rotation angles in radians! (stored in mm/radians in Ensenso tree)
270  * @note If a calibration has been stored in the EEPROM, it is copied in the Link node at nxLib tree start.
271  * This method overwrites the Link node but does not write to the EEPROM.
272  *
273  * More information on the parameters can be found in [Link node](http://www.ensenso.de/manual/index.html?cameralink.htm)
274  * section of the Ensenso manual.
275  *
276  * The point cloud you get from the Ensenso is already transformed using this calibration matrix.
277  * Make sure it is the identity transformation if you want the original point cloud! (use @ref clearEEPROMExtrinsicCalibration)
278  * Use @ref storeEEPROMExtrinsicCalibration to permanently store this transformation */
279  bool
280  setExtrinsicCalibration (const Eigen::Affine3d &transformation,
281  const std::string target = "Hand");
282 
283  /** @brief Obtain the number of frames per second (FPS) */
284  float
285  getFramesPerSecond () const override;
286 
287  /** @brief Open TCP port to enable access via the [nxTreeEdit](http://www.ensenso.de/manual/software_components.htm) program.
288  * @param[in] port The port number
289  * @return True if successful, false otherwise */
290  bool
291  openTcpPort (const int port = 24000);
292 
293  /** @brief Close TCP port program
294  * @return True if successful, false otherwise
295  * @warning If you do not close the TCP port the program might exit with the port still open, if it is the case
296  * use @code ps -ef @endcode and @code kill PID @endcode to kill the application and effectively close the port. */
297  bool
298  closeTcpPort ();
299 
300  /** @brief Returns the full NxLib tree as a JSON string
301  * @param[in] pretty_format JSON formatting style
302  * @return A string containing the NxLib tree in JSON format */
303  std::string
304  getTreeAsJson (const bool pretty_format = true) const;
305 
306  /** @brief Returns the Result node (of the last command) as a JSON string
307  * @param[in] pretty_format JSON formatting style
308  * @return A string containing the Result node in JSON format
309  */
310  std::string
311  getResultAsJson (const bool pretty_format = true) const;
312 
313  /** @brief Get the Euler angles corresponding to a JSON string (an angle axis transformation)
314  * @param[in] json A string containing the angle axis transformation in JSON format
315  * @param[out] x The X translation
316  * @param[out] y The Y translation
317  * @param[out] z The Z translation
318  * @param[out] w The yaW angle
319  * @param[out] p The Pitch angle
320  * @param[out] r The Roll angle
321  * @return True if successful, false otherwise
322  * @warning The units are meters and radians!
323  * @note See: [transformation page](http://www.ensenso.de/manual/transformation.htm) in the EnsensoSDK documentation
324  */
325  bool
326  jsonTransformationToEulerAngles (const std::string &json,
327  double &x,
328  double &y,
329  double &z,
330  double &w,
331  double &p,
332  double &r) const;
333 
334  /** @brief Get the angle axis parameters corresponding to a JSON string
335  * @param[in] json A string containing the angle axis transformation in JSON format
336  * @param[out] alpha Euler angle
337  * @param[out] axis Axis vector
338  * @param[out] translation Translation vector
339  * @return True if successful, false otherwise
340  * @warning The units are meters and radians!
341  * @note See: [transformation page](http://www.ensenso.de/manual/transformation.htm) in the EnsensoSDK documentation
342  */
343  bool
344  jsonTransformationToAngleAxis (const std::string json,
345  double &alpha,
346  Eigen::Vector3d &axis,
347  Eigen::Vector3d &translation) const;
348 
349 
350  /** @brief Get the JSON string corresponding to a 4x4 matrix
351  * @param[in] transformation The input transformation
352  * @param[out] matrix A matrix containing JSON transformation
353  * @return True if successful, false otherwise
354  * @warning The units are meters and radians!
355  * @note See: [ConvertTransformation page](http://www.ensenso.de/manual/index.html?cmdconverttransformation.htm) in the EnsensoSDK documentation
356  */
357  bool
358  jsonTransformationToMatrix (const std::string transformation,
359  Eigen::Affine3d &matrix) const;
360 
361 
362  /** @brief Get the JSON string corresponding to the Euler angles transformation
363  * @param[in] x The X translation
364  * @param[in] y The Y translation
365  * @param[in] z The Z translation
366  * @param[in] w The yaW angle
367  * @param[in] p The Pitch angle
368  * @param[in] r The Roll angle
369  * @param[out] json A string containing the Euler angles transformation in JSON format
370  * @param[in] pretty_format JSON formatting style
371  * @return True if successful, false otherwise
372  * @warning The units are meters and radians!
373  * @note See: [transformation page](http://www.ensenso.de/manual/transformation.htm) in the EnsensoSDK documentation
374  */
375  bool
376  eulerAnglesTransformationToJson (const double x,
377  const double y,
378  const double z,
379  const double w,
380  const double p,
381  const double r,
382  std::string &json,
383  const bool pretty_format = true) const;
384 
385  /** @brief Get the JSON string corresponding to an angle axis transformation
386  * @param[in] x The X angle
387  * @param[in] y The Y angle
388  * @param[in] z The Z angle
389  * @param[in] rx The X component of the Euler axis
390  * @param[in] ry The Y component of the Euler axis
391  * @param[in] rz The Z component of the Euler axis
392  * @param[in] alpha The Euler rotation angle
393  * @param[out] json A string containing the angle axis transformation in JSON format
394  * @param[in] pretty_format JSON formatting style
395  * @return True if successful, false otherwise
396  * @warning The units are meters and radians! (the Euler axis doesn't need to be normalized)
397  * @note See: [transformation page](http://www.ensenso.de/manual/transformation.htm) in the EnsensoSDK documentation
398  */
399  bool
400  angleAxisTransformationToJson (const double x,
401  const double y,
402  const double z,
403  const double rx,
404  const double ry,
405  const double rz,
406  const double alpha,
407  std::string &json,
408  const bool pretty_format = true) const;
409 
410  /** @brief Get the JSON string corresponding to a 4x4 matrix
411  * @param[in] matrix The input matrix
412  * @param[out] json A string containing the matrix transformation in JSON format
413  * @param[in] pretty_format JSON formatting style
414  * @return True if successful, false otherwise
415  * @warning The units are meters and radians!
416  * @note See: [ConvertTransformation page](http://www.ensenso.de/manual/index.html?cmdconverttransformation.htm)
417  * in the EnsensoSDK documentation */
418  bool
419  matrixTransformationToJson (const Eigen::Affine3d &matrix,
420  std::string &json,
421  const bool pretty_format = true) const;
422 
423  /** @brief Reference to the NxLib tree root
424  * @warning You must handle NxLib exceptions manually when playing with @ref root_ !
425  * See ensensoExceptionHandling in ensenso_grabber.cpp */
426  shared_ptr<const NxLibItem> root_;
427 
428  /** @brief Reference to the camera tree
429  * @warning You must handle NxLib exceptions manually when playing with @ref camera_ ! */
430  NxLibItem camera_;
431 
432  protected:
433  /** @brief Grabber thread */
434  std::thread grabber_thread_;
435 
436  /** @brief Boost point cloud signal */
437  boost::signals2::signal<sig_cb_ensenso_point_cloud>* point_cloud_signal_;
438 
439  /** @brief Boost images signal */
440  boost::signals2::signal<sig_cb_ensenso_images>* images_signal_;
441 
442  /** @brief Boost images + point cloud signal */
443  boost::signals2::signal<sig_cb_ensenso_point_cloud_images>* point_cloud_images_signal_;
444 
445  /** @brief Whether an Ensenso device is opened or not */
446  bool device_open_{false};
447 
448  /** @brief Whether an TCP port is opened or not */
449  bool tcp_open_{false};
450 
451  /** @brief Whether an Ensenso device is running or not */
452  bool running_{false};
453 
454  /** @brief Point cloud capture/processing frequency */
456 
457  /** @brief Mutual exclusion for FPS computation */
458  mutable std::mutex fps_mutex_;
459 
460  /** @brief Convert an Ensenso time stamp into a PCL/ROS time stamp
461  * @param[in] ensenso_stamp
462  * @return PCL stamp
463  * The Ensenso API returns the time elapsed from January 1st, 1601 (UTC); on Linux OS the reference time is January 1st, 1970 (UTC).
464  * See [time-stamp page](http://www.ensenso.de/manual/index.html?json_types.htm) for more info about the time stamp conversion. */
465  std::uint64_t
466  static
467  getPCLStamp (const double ensenso_stamp);
468 
469  /** @brief Get OpenCV image type corresponding to the parameters given
470  * @param channels number of channels in the image
471  * @param bpe bytes per element
472  * @param isFlt is float
473  * @return the OpenCV type as a string */
474  std::string
475  static
476  getOpenCVType (const int channels,
477  const int bpe,
478  const bool isFlt);
479 
480  /** @brief Continuously asks for images and or point clouds data from the device and publishes them if available.
481  * PCL time stamps are filled for both the images and clouds grabbed (see @ref getPCLStamp)
482  * @note The cloud time stamp is the RAW image time stamp */
483  void
485  };
486 } // namespace pcl
Grabber for IDS-Imaging Ensenso's devices.
static std::uint64_t getPCLStamp(const double ensenso_stamp)
Convert an Ensenso time stamp into a PCL/ROS time stamp.
static std::string getOpenCVType(const int channels, const int bpe, const bool isFlt)
Get OpenCV image type corresponding to the parameters given.
void processGrabbing()
Continuously asks for images and or point clouds data from the device and publishes them if available...
~EnsensoGrabber() noexcept override
Destructor inherited from the Grabber interface.
pcl::EventFrequency frequency_
Point cloud capture/processing frequency.
EnsensoGrabber()
Constructor.
std::mutex fps_mutex_
Mutual exclusion for FPS computation.
A helper class to measure frequency of a certain event.
Definition: time.h:133
Grabber interface for PCL 1.x device drivers.
Definition: grabber.h:60
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
Define methods for measuring time spent in code blocks.
Definition: bfgs.h:10
#define PCL_EXPORTS
Definition: pcl_macros.h:323
A point structure representing Euclidean xyz coordinates.