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