Main MRPT website > C++ reference for MRPT 1.5.7
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 }
Declares a class derived from "CObservation" that encapsules an omnidirectional RGBD measurement from...
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
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.
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:29
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
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)
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)
Contains classes for various device interfaces.
mrpt::system::TTimeStamp timestamps[NUM_SENSORS]
STL namespace.
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.
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
#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)...
GLint GLvoid * img
Definition: glext.h:3645
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) ...
This namespace contains representation of robot actions and observations.
This namespace provides multitask, synchronization utilities.
virtual void initialize()
Initializes the 3D camera - should be invoked after calling loadConfig() or setting the different par...
#define DEG2RAD
GLsizei const GLchar ** string
Definition: glext.h:3919
#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
static CDisplayWindowPtr Create()
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp. Where available, this should contain the accurate satellite-based time...
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
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
bool hasRangeImage
true means the field rangeImage contains valid data



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019