MRPT  1.9.9
COpenNI2_RGBD360.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://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;
24 using mrpt::DEG2RAD;
27 
29 
30 /*-------------------------------------------------------------
31 ctor
32 -------------------------------------------------------------*/
33 COpenNI2_RGBD360::COpenNI2_RGBD360() : m_sensorPoseOnRobot()
34 
35 {
36  // Default label:
37  m_sensorLabel = "RGBD360";
38 }
39 
40 /*-------------------------------------------------------------
41 dtor
42 -------------------------------------------------------------*/
44 {
45 #if MRPT_HAS_OPENNI2
46  kill();
47 #endif // MRPT_HAS_OPENNI2
48 }
49 
50 /** This method can or cannot be implemented in the derived class, depending on
51  * the need for it.
52  * \exception This method must throw an exception with a descriptive message if
53  * some critical error is found.
54  */
56 {
57 #if MRPT_HAS_OPENNI2
58  // Check and list the available devices
59  getConnectedDevices();
60 
61  if (getNumDevices() < NUM_SENSORS)
62  {
63  cout << "Num required sensors " << NUM_SENSORS << endl;
64  cout << "Not enough devices connected -> EXIT\n";
65  return;
66  }
67  cout << "COpenNI2_RGBD360 initializes correctly.\n";
68 
69  for (unsigned sensor_id = 0; sensor_id < (unsigned int)NUM_SENSORS;
70  sensor_id++)
71  open(sensor_id);
72 #else
73  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
74 #endif // MRPT_HAS_OPENNI2
75 }
76 
77 /** This method will be invoked at a minimum rate of "process_rate" (Hz)
78  * \exception This method must throw an exception with a descriptive message if
79  * some critical error is found.
80  */
82 {
83 #if MRPT_HAS_OPENNI2
84  cout << "COpenNI2_RGBD360::doProcess...\n";
85 
86  bool thereIs, hwError;
87 
88  CObservationRGBD360::Ptr newObs = std::make_shared<CObservationRGBD360>();
89  // CObservation3DRangeScan::Ptr newObs =
90  // std::make_shared<CObservation3DRangeScan>();
91 
92  assert(getNumDevices() > 0);
93  // unsigned sensor_id = COpenNI2Generic::vOpenDevices.front();
94  getNextObservation(*newObs, thereIs, hwError);
95 
96  if (hwError)
97  {
98  m_state = ssError;
99  THROW_EXCEPTION("Couldn't communicate to the OpenNI2 sensor!");
100  }
101 
102  if (thereIs)
103  {
104  m_state = ssWorking;
105 
106  std::vector<mrpt::serialization::CSerializable::Ptr> objs;
107  if (m_grab_rgb || m_grab_depth || m_grab_3D_points)
108  objs.push_back(newObs);
109 
110  appendObservations(objs);
111  }
112 #else
113  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
114 #endif // MRPT_HAS_OPENNI2
115 }
116 
117 /** Loads specific configuration for the device from a given source of
118  * configuration parameters, for example, an ".ini" file, loading from the
119  * section "[iniSection]" (see config::CConfigFileBase and derived classes)
120  * \exception This method must throw an exception with a descriptive message if
121  * some critical parameter is missing or has an invalid value.
122  */
124  const mrpt::config::CConfigFileBase& configSource,
125  const std::string& iniSection)
126 {
127  cout << "COpenNI2_RGBD360::loadConfig_sensorSpecific...\n";
128 
129  m_sensorPoseOnRobot.setFromValues(
130  configSource.read_float(iniSection, "pose_x", 0),
131  configSource.read_float(iniSection, "pose_y", 0),
132  configSource.read_float(iniSection, "pose_z", 0),
133  DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
134  DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
135  DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));
136 
137  m_preview_window =
138  configSource.read_bool(iniSection, "preview_window", m_preview_window);
139 
140  m_width = configSource.read_int(iniSection, "width", 0);
141  m_height = configSource.read_int(iniSection, "height", 0);
142  m_fps = configSource.read_float(iniSection, "fps", 0);
143  std::cout << "width " << m_width << " height " << m_height << " fps "
144  << m_fps << endl;
145 
146  m_grab_rgb = configSource.read_bool(iniSection, "grab_image", m_grab_rgb);
147  m_grab_depth =
148  configSource.read_bool(iniSection, "grab_depth", m_grab_depth);
149  m_grab_3D_points =
150  configSource.read_bool(iniSection, "grab_3D_points", m_grab_3D_points);
151 
152  // m_num_sensors = configSource.read_int(iniSection,"m_num_sensors",0);
153 }
154 
155 /** The main data retrieving function, to be called after calling loadConfig()
156  * and initialize().
157  * \param out_obs The output retrieved observation (only if there_is_obs=true).
158  * \param there_is_obs If set to false, there was no new observation.
159  * \param hardware_error True on hardware/comms error.
160  *
161  * \sa doProcess
162  */
164  CObservationRGBD360& out_obs, bool& there_is_obs, bool& hardware_error)
165 {
166 #if MRPT_HAS_OPENNI2
167  CTicTac tictac;
168  tictac.Tic();
169  // cout << "COpenNI2_RGBD360::getNextObservation \n";
170 
171  there_is_obs = false;
172  hardware_error = false;
173 
174  CObservationRGBD360 newObs;
175  // Set intensity image ----------------------
176  if (m_grab_rgb) newObs.hasIntensityImage = true;
177  // Set range image --------------------------
178  if (m_grab_depth || m_grab_3D_points) newObs.hasRangeImage = true;
179 
181 
182  for (unsigned sensor_id = 0; sensor_id < (unsigned int)NUM_SENSORS;
183  sensor_id++)
184  {
185  getNextFrameRGB(
186  newObs.intensityImages[sensor_id], newObs.timestamps[sensor_id],
187  there_is_obs, hardware_error, sensor_id);
188  getNextFrameD(
189  newObs.rangeImages[sensor_id], newObs.timestamps[sensor_id],
190  there_is_obs, hardware_error, sensor_id);
191  }
192 
193  // preview in real-time?
194  for (unsigned sensor_id = 0; sensor_id < (unsigned int)NUM_SENSORS;
195  sensor_id++)
196  {
197  if (m_preview_window)
198  {
199  if (out_obs.hasRangeImage)
200  {
201  if (++m_preview_decim_counter_range >
202  m_preview_window_decimation)
203  {
204  m_preview_decim_counter_range = 0;
205  if (!m_win_range[sensor_id])
206  {
207  m_win_range[sensor_id] =
208  mrpt::gui::CDisplayWindow::Create("Preview RANGE");
209  m_win_range[sensor_id]->setPos(5, 5 + 250 * sensor_id);
210  }
211 
212  // Normalize the image
214  img.setFromMatrix(out_obs.rangeImages[sensor_id]);
215  CMatrixFloat r = out_obs.rangeImages[sensor_id];
216  r *= float(1.0 / this->m_maxRange);
217  m_win_range[sensor_id]->showImage(img);
218  }
219  }
220  if (out_obs.hasIntensityImage)
221  {
222  if (++m_preview_decim_counter_rgb > m_preview_window_decimation)
223  {
224  m_preview_decim_counter_rgb = 0;
225  if (!m_win_int[sensor_id])
226  {
227  m_win_int[sensor_id] =
229  "Preview INTENSITY");
230  m_win_int[sensor_id]->setPos(330, 5 + 250 * sensor_id);
231  }
232  m_win_int[sensor_id]->showImage(
233  out_obs.intensityImages[sensor_id]);
234  }
235  }
236  }
237  else
238  {
239  if (m_win_range[sensor_id]) m_win_range[sensor_id].reset();
240  if (m_win_int[sensor_id]) m_win_int[sensor_id].reset();
241  }
242  }
243  cout << "getNextObservation took " << 1000 * tictac.Tac() << "ms\n";
244 #else
245  MRPT_UNUSED_PARAM(out_obs);
246  MRPT_UNUSED_PARAM(there_is_obs);
247  MRPT_UNUSED_PARAM(hardware_error);
248  MRPT_UNUSED_PARAM(out_obs);
249  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
250 #endif // MRPT_HAS_OPENNI2
251 }
252 
253 /* -----------------------------------------------------
254 setPathForExternalImages
255 ----------------------------------------------------- */
257 {
258  MRPT_UNUSED_PARAM(directory);
259  // Ignore for now. It seems performance is better grabbing everything
260  // to a single big file than creating hundreds of smaller files per
261  // second...
262  return;
263 
264  // if (!mrpt::system::createDirectory( directory ))
265  // {
266  // THROW_EXCEPTION_FMT("Error: Cannot create the directory for
267  // externally
268  // saved images: %s",directory.c_str() )
269  // }
270  // m_path_for_external_images = directory;
271 }
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:86
Declares a class derived from "CObservation" that encapsules an omnidirectional RGBD measurement from...
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
mrpt::system::TTimeStamp getCurrentTime()
Returns the current (UTC) system time.
Definition: datetime.h:82
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.
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:5
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.
mrpt::math::CMatrixF rangeImages[NUM_SENSORS]
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters) ...
GLint GLvoid * img
Definition: glext.h:3769
This namespace contains representation of robot actions and observations.
GLsizei const GLchar ** string
Definition: glext.h:4116
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
bool hasIntensityImage
true means the field intensityImage contains valid data
~COpenNI2_RGBD360() override
Default ctor.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
Definition: CObservation.h:60
GLdouble GLdouble GLdouble r
Definition: glext.h:3711
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().
void initialize() override
Initializes the 3D camera - should be invoked after calling loadConfig() or setting the different par...
static CDisplayWindow::Ptr Create(const std::string &windowCaption, unsigned int initWidth=400, unsigned int initHeight=400)
Class factory returning a smart pointer.
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:75
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &section) override
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)
This template class provides the basic functionality for a general 2D any-size, resizable container o...
void doProcess() override
To be called at a high rate (>XX Hz), this method populates the internal buffer of received observati...
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:147
bool hasRangeImage
true means the field rangeImage contains valid data
void setPathForExternalImages(const std::string &directory) override
Set the path where to save off-rawlog image files (this class DOES take into account this path)...
#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: 1de0e027c Sat Sep 14 16:15:22 2019 +0200 at sáb sep 14 16:20:14 CEST 2019