Point Cloud Library (PCL)  1.10.0
real_sense_grabber.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2015-, 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  */
37 
38 #pragma once
39 
40 #include <pcl/io/grabber.h>
41 #include <pcl/make_shared.h>
42 #include <pcl/point_cloud.h>
43 #include <pcl/point_types.h>
44 #include <pcl/common/time.h>
45 
46 #include <memory>
47 #include <thread>
48 
49 namespace pcl
50 {
51 
52  namespace io
53  {
54 
55  template <typename T> class Buffer;
56 
57  namespace real_sense
58  {
59  class RealSenseDevice;
60  }
61 
62  }
63 
65  {
66 
67  public:
68 
71 
74 
75  /** A descriptor for capturing mode.
76  *
77  * Consists of framerate and resolutions of depth and color streams.
78  * Serves two purposes: to describe the desired capturing mode when
79  * creating a grabber, and to list the available modes supported by the
80  * grabber (see getAvailableModes()). In the first case setting some
81  * fields to zero means "don't care", i.e. the grabber is allowed to
82  * decide itself which concrete values to use. */
84  {
85  unsigned int fps;
86  unsigned int depth_width;
87  unsigned int depth_height;
88  unsigned int color_width;
89  unsigned int color_height;
90 
91  /** Set all fields to zero (i.e. "don't care"). */
92  Mode ();
93 
94  /** Set desired framerate, the rest is "don't care". */
95  Mode (unsigned int fps);
96 
97  /** Set desired depth resolution, the rest is "don't care". */
98  Mode (unsigned int depth_width, unsigned int depth_height);
99 
100  /** Set desired framerate and depth resolution, the rest is "don't
101  * care". */
102  Mode (unsigned int fps, unsigned int depth_width, unsigned int depth_height);
103 
104  /** Set desired depth and color resolution, the rest is "don't
105  * care". */
106  Mode (unsigned int depth_width, unsigned int depth_height, unsigned int color_width, unsigned int color_height);
107 
108  /** Set desired framerate, depth and color resolution. */
109  Mode (unsigned int fps, unsigned int depth_width, unsigned int depth_height, unsigned int color_width, unsigned int color_height);
110 
111  bool
113  };
114 
116  {
117  RealSense_None = 0,
118  RealSense_Median = 1,
119  RealSense_Average = 2,
120  };
121 
122  /** Create a grabber for a RealSense device.
123  *
124  * The grabber "captures" the device, making it impossible for other
125  * grabbers to interact with it. The device is "released" when the
126  * grabber is destructed.
127  *
128  * This will throw pcl::io::IOException if there are no free devices
129  * that match the supplied \a device_id.
130  *
131  * \param[in] device_id device identifier, which can be a serial number,
132  * a zero-based index (with '#' prefix), or an empty string (to select
133  * the first available device)
134  * \param[in] mode desired framerate and stream resolution (see Mode).
135  * If the default is supplied, then the mode closest to VGA at 30 Hz
136  * will be chosen.
137  * \param[in] strict if set to \c true, an exception will be thrown if
138  * device does not support exactly the mode requested. Otherwise the
139  * closest available mode is selected. */
140  RealSenseGrabber (const std::string& device_id = "", const Mode& mode = Mode (), bool strict = false);
141 
142  virtual
143  ~RealSenseGrabber () throw ();
144 
145  virtual void
146  start ();
147 
148  virtual void
149  stop ();
150 
151  virtual bool
152  isRunning () const;
153 
154  virtual std::string
155  getName () const
156  {
157  return (std::string ("RealSenseGrabber"));
158  }
159 
160  virtual float
161  getFramesPerSecond () const;
162 
163  /** Set the confidence threshold for depth data.
164  *
165  * Valid range is [0..15]. Discarded points will have their coordinates
166  * set to NaNs). */
167  void
168  setConfidenceThreshold (unsigned int threshold);
169 
170  /** Enable temporal filtering of the depth data received from the device.
171  *
172  * The window size parameter is not relevant for `RealSense_None`
173  * filtering type.
174  *
175  * \note if the grabber is running and the new parameters are different
176  * from the current parameters, grabber will be restarted. */
177  void
178  enableTemporalFiltering (TemporalFilteringType type, std::size_t window_size);
179 
180  /** Disable temporal filtering. */
181  void
182  disableTemporalFiltering ();
183 
184  /** Get the serial number of device captured by the grabber. */
185  const std::string&
186  getDeviceSerialNumber () const;
187 
188  /** Get a list of capturing modes supported by the PXC device
189  * controlled by this grabber.
190  *
191  * \param[in] only_depth list depth-only modes
192  *
193  * \note: this list exclude modes where framerates of the depth and
194  * color streams do not match. */
195  std::vector<Mode>
196  getAvailableModes (bool only_depth = false) const;
197 
198  /** Set desired capturing mode.
199  *
200  * \note if the grabber is running and the new mode is different the
201  * one requested previously, grabber will be restarted. */
202  void
203  setMode (const Mode& mode, bool strict = false);
204 
205  /** Get currently active capturing mode.
206  *
207  * \note: capturing mode is selected when start() is called; output of
208  * this function before grabber was started is undefined. */
209  const Mode&
210  getMode () const
211  {
212  return (mode_selected_);
213  }
214 
215  private:
216 
217  void
218  run ();
219 
220  void
221  createDepthBuffer ();
222 
223  void
224  selectMode ();
225 
226  /** Compute a score which indicates how different is a given mode is from
227  * the mode requested by the user.
228  *
229  * Importance of factors: fps > depth resolution > color resolution. The
230  * lower the score the better. */
231  float
232  computeModeScore (const Mode& mode);
233 
234  // Signals to indicate whether new clouds are available
235  boost::signals2::signal<sig_cb_real_sense_point_cloud>* point_cloud_signal_;
236  boost::signals2::signal<sig_cb_real_sense_point_cloud_rgba>* point_cloud_rgba_signal_;
237 
238  std::shared_ptr<pcl::io::real_sense::RealSenseDevice> device_;
239 
240  bool is_running_;
241  unsigned int confidence_threshold_;
242 
243  TemporalFilteringType temporal_filtering_type_;
244  std::size_t temporal_filtering_window_size_;
245 
246  /// Capture mode requested by the user at construction time
247  Mode mode_requested_;
248 
249  /// Whether or not selected capture mode should strictly match what the user
250  /// has requested
251  bool strict_;
252 
253  /// Capture mode selected by grabber (among the modes supported by the
254  /// device), computed and stored on start()
255  Mode mode_selected_;
256 
257  /// Indicates whether there are subscribers for PointXYZ signal, computed
258  /// and stored on start()
259  bool need_xyz_;
260 
261  /// Indicates whether there are subscribers for PointXYZRGBA signal,
262  /// computed and stored on start()
263  bool need_xyzrgba_;
264 
265  EventFrequency frequency_;
266  mutable std::mutex fps_mutex_;
267 
268  std::thread thread_;
269 
270  /// Depth buffer to perform temporal filtering of the depth images
271  std::shared_ptr<pcl::io::Buffer<unsigned short> > depth_buffer_;
272 
273  };
274 
275 }
pcl::RealSenseGrabber::Mode
A descriptor for capturing mode.
Definition: real_sense_grabber.h:83
pcl::RealSenseGrabber::ConstPtr
shared_ptr< const RealSenseGrabber > ConstPtr
Definition: real_sense_grabber.h:70
pcl
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
point_types.h
pcl::RealSenseGrabber
Definition: real_sense_grabber.h:64
pcl::RealSenseGrabber::Ptr
shared_ptr< RealSenseGrabber > Ptr
Definition: real_sense_grabber.h:69
pcl::Grabber
Grabber interface for PCL 1.x device drivers.
Definition: grabber.h:57
pcl::RealSenseGrabber::Mode::color_width
unsigned int color_width
Definition: real_sense_grabber.h:88
pcl::RealSenseGrabber::getMode
const Mode & getMode() const
Get currently active capturing mode.
Definition: real_sense_grabber.h:210
pcl::RealSenseGrabber::Mode::depth_width
unsigned int depth_width
Definition: real_sense_grabber.h:86
pcl::EventFrequency
A helper class to measure frequency of a certain event.
Definition: time.h:134
pcl::RealSenseGrabber::Mode::fps
unsigned int fps
Definition: real_sense_grabber.h:85
pcl::RealSenseGrabber::sig_cb_real_sense_point_cloud_rgba
void(const pcl::PointCloud< pcl::PointXYZRGBA >::ConstPtr &) sig_cb_real_sense_point_cloud_rgba
Definition: real_sense_grabber.h:73
pcl::RealSenseGrabber::TemporalFilteringType
TemporalFilteringType
Definition: real_sense_grabber.h:115
time.h
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:416
pcl::RealSenseGrabber::Mode::color_height
unsigned int color_height
Definition: real_sense_grabber.h:89
pcl::operator==
bool operator==(const PCLHeader &lhs, const PCLHeader &rhs)
Definition: PCLHeader.h:42
pcl::RealSenseGrabber::sig_cb_real_sense_point_cloud
void(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &) sig_cb_real_sense_point_cloud
Definition: real_sense_grabber.h:72
PCL_EXPORTS
#define PCL_EXPORTS
Definition: pcl_macros.h:253
pcl::RealSenseGrabber::Mode::depth_height
unsigned int depth_height
Definition: real_sense_grabber.h:87
pcl::shared_ptr
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
Definition: pcl_macros.h:90