Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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 
14 #include <mrpt/utils/CTimeLogger.h>
15 #include <mrpt/system/threads.h>
16 
17 // Universal include for all versions of OpenCV
18 #include <mrpt/otherlibs/do_opencv_includes.h>
19 
20 using namespace mrpt::hwdrivers;
21 using namespace mrpt::system;
22 using namespace mrpt::obs;
23 using namespace mrpt::synch;
24 using namespace mrpt::math;
25 using namespace std;
29 
31 
32 /*-------------------------------------------------------------
33 ctor
34 -------------------------------------------------------------*/
36  m_sensorPoseOnRobot(),
37  m_preview_window(false),
38  m_preview_window_decimation(1),
39  m_preview_decim_counter_range(0),
40  m_preview_decim_counter_rgb(0),
41 
42  m_grab_rgb(true),
43  m_grab_depth(true),
44  m_grab_3D_points(true)
45 {
46 
47  // Default label:
48  m_sensorLabel = "RGBD360";
49 
50 }
51 
52 /*-------------------------------------------------------------
53 dtor
54 -------------------------------------------------------------*/
56 {
57 #if MRPT_HAS_OPENNI2
58  kill();
59 #endif // MRPT_HAS_OPENNI2
60 }
61 
62 /** This method can or cannot be implemented in the derived class, depending on the need for it.
63 * \exception This method must throw an exception with a descriptive message if some critical error is found.
64 */
66 {
67 #if MRPT_HAS_OPENNI2
68  // Check and list the available devices
69  getConnectedDevices();
70 
71  if(getNumDevices() < NUM_SENSORS)
72  {
73  cout << "Num required sensors " << NUM_SENSORS << endl;
74  cout << "Not enough devices connected -> EXIT\n";
75  return;
76  }
77  cout << "COpenNI2_RGBD360 initializes correctly.\n";
78 
79  for(unsigned sensor_id=0; sensor_id < (unsigned int)NUM_SENSORS; sensor_id++)
80  open(sensor_id);
81 #else
82  THROW_EXCEPTION("MRPT was built without OpenNI2 support")
83 #endif // MRPT_HAS_OPENNI2
84 }
85 
86 /** This method will be invoked at a minimum rate of "process_rate" (Hz)
87 * \exception This method must throw an exception with a descriptive message if some critical error is found.
88 */
90 {
91 #if MRPT_HAS_OPENNI2
92  cout << "COpenNI2_RGBD360::doProcess...\n";
93 
94  bool thereIs, hwError;
95 
96  CObservationRGBD360Ptr newObs = CObservationRGBD360::Create();
97  // CObservation3DRangeScanPtr newObs = CObservation3DRangeScan::Create();
98 
99  assert(getNumDevices() > 0);
100  // unsigned sensor_id = COpenNI2Generic::vOpenDevices.front();
101  getNextObservation( *newObs, thereIs, hwError );
102 
103  if (hwError)
104  {
105  m_state = ssError;
106  THROW_EXCEPTION("Couldn't communicate to the OpenNI2 sensor!");
107  }
108 
109  if (thereIs)
110  {
111  m_state = ssWorking;
112 
113  std::vector<mrpt::utils::CSerializablePtr> objs;
114  if (m_grab_rgb || m_grab_depth || m_grab_3D_points) objs.push_back(newObs);
115 
116  appendObservations( objs );
117  }
118 #else
119  THROW_EXCEPTION("MRPT was built without OpenNI2 support")
120 #endif // MRPT_HAS_OPENNI2
121 }
122 
123 /** Loads specific configuration for the device from a given source of configuration parameters, for example, an ".ini" file, loading from the section "[iniSection]" (see utils::CConfigFileBase and derived classes)
124 * \exception This method must throw an exception with a descriptive message if some critical parameter is missing or has an invalid value.
125 */
127  const mrpt::utils::CConfigFileBase &configSource,
128  const std::string &iniSection )
129 {
130  cout << "COpenNI2_RGBD360::loadConfig_sensorSpecific...\n";
131 
132  m_sensorPoseOnRobot.setFromValues(
133  configSource.read_float(iniSection,"pose_x",0),
134  configSource.read_float(iniSection,"pose_y",0),
135  configSource.read_float(iniSection,"pose_z",0),
136  DEG2RAD( configSource.read_float(iniSection,"pose_yaw",0) ),
137  DEG2RAD( configSource.read_float(iniSection,"pose_pitch",0) ),
138  DEG2RAD( configSource.read_float(iniSection,"pose_roll",0) )
139  );
140 
141  m_preview_window = configSource.read_bool(iniSection,"preview_window",m_preview_window);
142 
143  m_width = configSource.read_int(iniSection,"width",0);
144  m_height = configSource.read_int(iniSection,"height",0);
145  m_fps = configSource.read_float(iniSection,"fps",0);
146  std::cout << "width " << m_width << " height " << m_height << " fps " << m_fps << endl;
147 
148  m_grab_rgb = configSource.read_bool(iniSection,"grab_image",m_grab_rgb);
149  m_grab_depth = configSource.read_bool(iniSection,"grab_depth",m_grab_depth);
150  m_grab_3D_points = 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 
156 /** The main data retrieving function, to be called after calling loadConfig() 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,
165  bool &there_is_obs,
166  bool &hardware_error )
167 {
168 #if MRPT_HAS_OPENNI2
169  CTicTac tictac;
170  tictac.Tic();
171 // cout << "COpenNI2_RGBD360::getNextObservation \n";
172 
173  there_is_obs=false;
174  hardware_error = false;
175 
176  CObservationRGBD360 newObs;
177  // Set intensity image ----------------------
178  if (m_grab_rgb)
179  newObs.hasIntensityImage = true;
180  // Set range image --------------------------
181  if (m_grab_depth || m_grab_3D_points)
182  newObs.hasRangeImage = true;
183 
185 
186  for(unsigned sensor_id=0; sensor_id < (unsigned int)NUM_SENSORS; sensor_id++)
187  {
188 // cout << "Get sensor " << sensor_id << " \n";
189  bool there_is_obs, hardware_error;
190  getNextFrameRGB(newObs.intensityImages[sensor_id],newObs.timestamps[sensor_id], there_is_obs, hardware_error, sensor_id);
191  getNextFrameD(newObs.rangeImages[sensor_id],newObs.timestamps[sensor_id], there_is_obs, hardware_error, sensor_id);
192  }
193 
194 
195  // preview in real-time?
196  for(unsigned sensor_id=0; sensor_id < (unsigned int)NUM_SENSORS; sensor_id++) {
197  if (m_preview_window)
198  {
199  if ( out_obs.hasRangeImage )
200  {
201  if (++m_preview_decim_counter_range>m_preview_window_decimation)
202  {
203  m_preview_decim_counter_range=0;
204  if (!m_win_range[sensor_id]) { m_win_range[sensor_id] = mrpt::gui::CDisplayWindow::Create("Preview RANGE"); m_win_range[sensor_id]->setPos(5,5+250*sensor_id); }
205 
206  // Normalize the image
208  img.setFromMatrix(out_obs.rangeImages[sensor_id]);
209  CMatrixFloat r = out_obs.rangeImages[sensor_id] * float(1.0/this->m_maxRange);
210  m_win_range[sensor_id]->showImage(img);
211  }
212  }
213  if ( out_obs.hasIntensityImage )
214  {
215  if (++m_preview_decim_counter_rgb>m_preview_window_decimation)
216  {
217  m_preview_decim_counter_rgb=0;
218  if (!m_win_int[sensor_id]) { m_win_int[sensor_id] = mrpt::gui::CDisplayWindow::Create("Preview INTENSITY"); m_win_int[sensor_id]->setPos(330,5+250*sensor_id); }
219  m_win_int[sensor_id]->showImage(out_obs.intensityImages[sensor_id] );
220  }
221  }
222  }
223  else
224  {
225  if (m_win_range[sensor_id]) m_win_range[sensor_id].clear();
226  if (m_win_int[sensor_id]) m_win_int[sensor_id].clear();
227  }
228  }
229  cout << "getNextObservation took " << 1000*tictac.Tac() << "ms\n";
230 #else
231  MRPT_UNUSED_PARAM(out_obs); MRPT_UNUSED_PARAM(there_is_obs); MRPT_UNUSED_PARAM(hardware_error);
232  MRPT_UNUSED_PARAM(out_obs);
233  THROW_EXCEPTION("MRPT was built without OpenNI2 support")
234 #endif // MRPT_HAS_OPENNI2
235 }
236 
237 
238 /* -----------------------------------------------------
239 setPathForExternalImages
240 ----------------------------------------------------- */
242 {
243  MRPT_UNUSED_PARAM(directory);
244  // Ignore for now. It seems performance is better grabbing everything
245  // to a single big file than creating hundreds of smaller files per second...
246  return;
247 
248  // if (!mrpt::system::createDirectory( directory ))
249  // {
250  // THROW_EXCEPTION_FMT("Error: Cannot create the directory for externally saved images: %s",directory.c_str() )
251  // }
252  // m_path_for_external_images = directory;
253 }
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
Declares a class derived from "CObservation" that encapsules an omnidirectional RGBD measurement from...
mrpt::utils::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.
virtual void loadConfig_sensorSpecific(const mrpt::utils::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 utils::CConfigFileBase and derived classes) method must throw an exception with a descriptive message if some critical parameter is missing or has an invalid value.
A class for storing images as grayscale or RGB bitmaps.
Definition: CImage.h:101
mrpt::system::TTimeStamp BASE_IMPEXP getCurrentTime()
Returns the current (UTC) system time.
Definition: datetime.cpp:71
#define THROW_EXCEPTION(msg)
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
mrpt::system::TTimeStamp timestamps[NUM_SENSORS]
virtual void doProcess()
To be called at a high rate (>XX Hz), this method populates the internal buffer of received observati...
void Tic()
Starts the stopwatch.
Definition: CTicTac.cpp:77
This class allows loading and storing values and vectors of different types from a configuration text...
A class for grabing RGBD images from several OpenNI2 sensors.
GLint GLvoid * img
Definition: glew.h:1290
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
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)...
This class implements a high-performance stopwatch.
Definition: CTicTac.h:24
mrpt::math::CMatrix rangeImages[NUM_SENSORS]
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters) ...
virtual void initialize()
Initializes the 3D camera - should be invoked after calling loadConfig() or setting the different par...
#define DEG2RAD
typedef int(WINAPI *PFNWGLRELEASEPBUFFERDCARBPROC)(HPBUFFERARB hPbuffer
#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
void setFromMatrix(const Eigen::MatrixBase< Derived > &m, bool matrix_is_normalized=true)
Set the image from a matrix, interpreted as grayscale intensity values, in the range [0...
Definition: CImage.h:746
static CDisplayWindowPtr Create()
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp. Where available, this should contain the accurate satellite-based time...
GLsizei const GLcharARB ** string
Definition: glew.h:3293
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().
double Tac()
Stops the stopwatch.
Definition: CTicTac.cpp:92
GLdouble GLdouble GLdouble r
Definition: glew.h:1311
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
bool hasRangeImage
true means the field rangeImage contains valid data



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018