Main MRPT website > C++ reference for MRPT 1.9.9
COpenNI2_RGBD360.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "hwdrivers-precomp.h" // Precompiled header
11 
15 
16 // Universal include for all versions of OpenCV
17 #include <mrpt/otherlibs/do_opencv_includes.h>
18 
19 using namespace mrpt::hwdrivers;
20 using namespace mrpt::system;
21 using namespace mrpt::obs;
22 using namespace mrpt::math;
23 using namespace std;
25 using mrpt::DEG2RAD;
27 
29 
30 /*-------------------------------------------------------------
31 ctor
32 -------------------------------------------------------------*/
34  : m_sensorPoseOnRobot(),
35  m_preview_window(false),
36  m_preview_window_decimation(1),
37  m_preview_decim_counter_range(0),
38  m_preview_decim_counter_rgb(0),
39 
40  m_grab_rgb(true),
41  m_grab_depth(true),
42  m_grab_3D_points(true)
43 {
44  // Default label:
45  m_sensorLabel = "RGBD360";
46 }
47 
48 /*-------------------------------------------------------------
49 dtor
50 -------------------------------------------------------------*/
52 {
53 #if MRPT_HAS_OPENNI2
54  kill();
55 #endif // MRPT_HAS_OPENNI2
56 }
57 
58 /** This method can or cannot be implemented in the derived class, depending on
59 * the need for it.
60 * \exception This method must throw an exception with a descriptive message if
61 * some critical error is found.
62 */
64 {
65 #if MRPT_HAS_OPENNI2
66  // Check and list the available devices
67  getConnectedDevices();
68 
69  if (getNumDevices() < NUM_SENSORS)
70  {
71  cout << "Num required sensors " << NUM_SENSORS << endl;
72  cout << "Not enough devices connected -> EXIT\n";
73  return;
74  }
75  cout << "COpenNI2_RGBD360 initializes correctly.\n";
76 
77  for (unsigned sensor_id = 0; sensor_id < (unsigned int)NUM_SENSORS;
78  sensor_id++)
79  open(sensor_id);
80 #else
81  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
82 #endif // MRPT_HAS_OPENNI2
83 }
84 
85 /** This method will be invoked at a minimum rate of "process_rate" (Hz)
86 * \exception This method must throw an exception with a descriptive message if
87 * some critical error is found.
88 */
90 {
91 #if MRPT_HAS_OPENNI2
92  cout << "COpenNI2_RGBD360::doProcess...\n";
93 
94  bool thereIs, hwError;
95 
97  mrpt::make_aligned_shared<CObservationRGBD360>();
98  // CObservation3DRangeScan::Ptr newObs =
99  // mrpt::make_aligned_shared<CObservation3DRangeScan>();
100 
101  assert(getNumDevices() > 0);
102  // unsigned sensor_id = COpenNI2Generic::vOpenDevices.front();
103  getNextObservation(*newObs, thereIs, hwError);
104 
105  if (hwError)
106  {
107  m_state = ssError;
108  THROW_EXCEPTION("Couldn't communicate to the OpenNI2 sensor!");
109  }
110 
111  if (thereIs)
112  {
113  m_state = ssWorking;
114 
115  std::vector<mrpt::serialization::CSerializable::Ptr> objs;
116  if (m_grab_rgb || m_grab_depth || m_grab_3D_points)
117  objs.push_back(newObs);
118 
119  appendObservations(objs);
120  }
121 #else
122  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
123 #endif // MRPT_HAS_OPENNI2
124 }
125 
126 /** Loads specific configuration for the device from a given source of
127 * configuration parameters, for example, an ".ini" file, loading from the
128 * section "[iniSection]" (see config::CConfigFileBase and derived classes)
129 * \exception This method must throw an exception with a descriptive message if
130 * some critical parameter is missing or has an invalid value.
131 */
133  const mrpt::config::CConfigFileBase& configSource,
134  const std::string& iniSection)
135 {
136  cout << "COpenNI2_RGBD360::loadConfig_sensorSpecific...\n";
137 
138  m_sensorPoseOnRobot.setFromValues(
139  configSource.read_float(iniSection, "pose_x", 0),
140  configSource.read_float(iniSection, "pose_y", 0),
141  configSource.read_float(iniSection, "pose_z", 0),
142  DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
143  DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
144  DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));
145 
146  m_preview_window =
147  configSource.read_bool(iniSection, "preview_window", m_preview_window);
148 
149  m_width = configSource.read_int(iniSection, "width", 0);
150  m_height = configSource.read_int(iniSection, "height", 0);
151  m_fps = configSource.read_float(iniSection, "fps", 0);
152  std::cout << "width " << m_width << " height " << m_height << " fps "
153  << m_fps << endl;
154 
155  m_grab_rgb = configSource.read_bool(iniSection, "grab_image", m_grab_rgb);
156  m_grab_depth =
157  configSource.read_bool(iniSection, "grab_depth", m_grab_depth);
158  m_grab_3D_points =
159  configSource.read_bool(iniSection, "grab_3D_points", m_grab_3D_points);
160 
161  // m_num_sensors = configSource.read_int(iniSection,"m_num_sensors",0);
162 }
163 
164 /** The main data retrieving function, to be called after calling loadConfig()
165 * and initialize().
166 * \param out_obs The output retrieved observation (only if there_is_obs=true).
167 * \param there_is_obs If set to false, there was no new observation.
168 * \param hardware_error True on hardware/comms error.
169 *
170 * \sa doProcess
171 */
173  CObservationRGBD360& out_obs, bool& there_is_obs, bool& hardware_error)
174 {
175 #if MRPT_HAS_OPENNI2
176  CTicTac tictac;
177  tictac.Tic();
178  // cout << "COpenNI2_RGBD360::getNextObservation \n";
179 
180  there_is_obs = false;
181  hardware_error = false;
182 
183  CObservationRGBD360 newObs;
184  // Set intensity image ----------------------
185  if (m_grab_rgb) newObs.hasIntensityImage = true;
186  // Set range image --------------------------
187  if (m_grab_depth || m_grab_3D_points) newObs.hasRangeImage = true;
188 
190 
191  for (unsigned sensor_id = 0; sensor_id < (unsigned int)NUM_SENSORS;
192  sensor_id++)
193  {
194  // cout << "Get sensor " << sensor_id << " \n";
195  bool there_is_obs, hardware_error;
196  getNextFrameRGB(
197  newObs.intensityImages[sensor_id], newObs.timestamps[sensor_id],
198  there_is_obs, hardware_error, sensor_id);
199  getNextFrameD(
200  newObs.rangeImages[sensor_id], newObs.timestamps[sensor_id],
201  there_is_obs, hardware_error, sensor_id);
202  }
203 
204  // preview in real-time?
205  for (unsigned sensor_id = 0; sensor_id < (unsigned int)NUM_SENSORS;
206  sensor_id++)
207  {
208  if (m_preview_window)
209  {
210  if (out_obs.hasRangeImage)
211  {
212  if (++m_preview_decim_counter_range >
213  m_preview_window_decimation)
214  {
215  m_preview_decim_counter_range = 0;
216  if (!m_win_range[sensor_id])
217  {
218  m_win_range[sensor_id] = mrpt::make_aligned_shared<
219  mrpt::gui::CDisplayWindow>("Preview RANGE");
220  m_win_range[sensor_id]->setPos(5, 5 + 250 * sensor_id);
221  }
222 
223  // Normalize the image
225  img.setFromMatrix(out_obs.rangeImages[sensor_id]);
226  CMatrixFloat r = out_obs.rangeImages[sensor_id] *
227  float(1.0 / this->m_maxRange);
228  m_win_range[sensor_id]->showImage(img);
229  }
230  }
231  if (out_obs.hasIntensityImage)
232  {
233  if (++m_preview_decim_counter_rgb > m_preview_window_decimation)
234  {
235  m_preview_decim_counter_rgb = 0;
236  if (!m_win_int[sensor_id])
237  {
238  m_win_int[sensor_id] = mrpt::make_aligned_shared<
239  mrpt::gui::CDisplayWindow>("Preview INTENSITY");
240  m_win_int[sensor_id]->setPos(330, 5 + 250 * sensor_id);
241  }
242  m_win_int[sensor_id]->showImage(
243  out_obs.intensityImages[sensor_id]);
244  }
245  }
246  }
247  else
248  {
249  if (m_win_range[sensor_id]) m_win_range[sensor_id].reset();
250  if (m_win_int[sensor_id]) m_win_int[sensor_id].reset();
251  }
252  }
253  cout << "getNextObservation took " << 1000 * tictac.Tac() << "ms\n";
254 #else
255  MRPT_UNUSED_PARAM(out_obs);
256  MRPT_UNUSED_PARAM(there_is_obs);
257  MRPT_UNUSED_PARAM(hardware_error);
258  MRPT_UNUSED_PARAM(out_obs);
259  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
260 #endif // MRPT_HAS_OPENNI2
261 }
262 
263 /* -----------------------------------------------------
264 setPathForExternalImages
265 ----------------------------------------------------- */
267 {
268  MRPT_UNUSED_PARAM(directory);
269  // Ignore for now. It seems performance is better grabbing everything
270  // to a single big file than creating hundreds of smaller files per
271  // second...
272  return;
273 
274  // if (!mrpt::system::createDirectory( directory ))
275  // {
276  // THROW_EXCEPTION_FMT("Error: Cannot create the directory for
277  // externally
278  // saved images: %s",directory.c_str() )
279  // }
280  // m_path_for_external_images = directory;
281 }
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:90
Declares a class derived from "CObservation" that encapsules an omnidirectional RGBD measurement from...
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:25
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
mrpt::system::TTimeStamp getCurrentTime()
Returns the current (UTC) system time.
Definition: datetime.cpp:74
mrpt::img::CImage intensityImages[NUM_SENSORS]
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage"...
double DEG2RAD(const double x)
Degrees to radians.
A high-performance stopwatch, with typical resolution of nanoseconds.
Contains classes for various device interfaces.
mrpt::system::TTimeStamp timestamps[NUM_SENSORS]
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
STL namespace.
virtual void doProcess()
To be called at a high rate (>XX Hz), this method populates the internal buffer of received observati...
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
mrpt::system::CTicTac CTicTac
Definition: utils/CTicTac.h:7
A class for grabing RGBD images from several OpenNI2 sensors.
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
virtual void setPathForExternalImages(const std::string &directory)
Set the path where to save off-rawlog image files (this class DOES take into account this path)...
GLint GLvoid * img
Definition: glext.h:3763
This class creates a window as a graphical user interface (GUI) for displaying images to the user...
mrpt::math::CMatrix rangeImages[NUM_SENSORS]
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters) ...
This namespace contains representation of robot actions and observations.
virtual void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &section)
Loads specific configuration for the device from a given source of configuration parameters, for example, an ".ini" file, loading from the section "[iniSection]" (see config::CConfigFileBase and derived classes)
virtual void initialize()
Initializes the 3D camera - should be invoked after calling loadConfig() or setting the different par...
GLsizei const GLchar ** string
Definition: glext.h:4101
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
void setPos(int x, int y) override
Changes the position of the window on the screen.
bool hasIntensityImage
true means the field intensityImage contains valid data
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
Definition: CObservation.h:60
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
void getNextObservation(mrpt::obs::CObservationRGBD360 &out_obs, bool &there_is_obs, bool &hardware_error)
The main data retrieving function, to be called after calling loadConfig() and initialize().
std::shared_ptr< T > make_aligned_shared(Args &&... args)
Creates a shared_ptr with aligned memory via aligned_allocator_cpp11<>.
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:79
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:130
bool hasRangeImage
true means the field rangeImage contains valid data
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019