MRPT  2.0.4
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-2020, 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/3rdparty/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  [[maybe_unused]] CObservationRGBD360& out_obs,
165  [[maybe_unused]] bool& there_is_obs, [[maybe_unused]] bool& hardware_error)
166 {
167 #if MRPT_HAS_OPENNI2
168  CTicTac tictac;
169  tictac.Tic();
170  // cout << "COpenNI2_RGBD360::getNextObservation \n";
171 
172  there_is_obs = false;
173  hardware_error = false;
174 
175  CObservationRGBD360 newObs;
176  // Set intensity image ----------------------
177  if (m_grab_rgb) newObs.hasIntensityImage = true;
178  // Set range image --------------------------
179  if (m_grab_depth || m_grab_3D_points) newObs.hasRangeImage = true;
180 
182 
183  for (unsigned sensor_id = 0; sensor_id < (unsigned int)NUM_SENSORS;
184  sensor_id++)
185  {
186  getNextFrameRGB(
187  newObs.intensityImages[sensor_id], newObs.timestamps[sensor_id],
188  there_is_obs, hardware_error, sensor_id);
189  getNextFrameD(
190  newObs.rangeImages[sensor_id], newObs.timestamps[sensor_id],
191  there_is_obs, hardware_error, sensor_id);
192  }
193 
194  // preview in real-time?
195  for (unsigned sensor_id = 0; sensor_id < (unsigned int)NUM_SENSORS;
196  sensor_id++)
197  {
198  if (m_preview_window)
199  {
200  if (out_obs.hasRangeImage)
201  {
202  if (++m_preview_decim_counter_range >
203  m_preview_window_decimation)
204  {
205  m_preview_decim_counter_range = 0;
206  if (!m_win_range[sensor_id])
207  {
208  m_win_range[sensor_id] =
209  mrpt::gui::CDisplayWindow::Create("Preview RANGE");
210  m_win_range[sensor_id]->setPos(5, 5 + 250 * sensor_id);
211  }
212 
213  // Normalize the image
214  mrpt::img::CImage img;
215  const Eigen::MatrixXf r =
216  out_obs.rangeImages[sensor_id].asEigen().cast<float>() *
217  out_obs.rangeUnits * float(1.0 / this->m_maxRange);
218  img.setFromMatrix(r, true /*normalized 0-1 */);
219  m_win_range[sensor_id]->showImage(img);
220  }
221  }
222  if (out_obs.hasIntensityImage)
223  {
224  if (++m_preview_decim_counter_rgb > m_preview_window_decimation)
225  {
226  m_preview_decim_counter_rgb = 0;
227  if (!m_win_int[sensor_id])
228  {
229  m_win_int[sensor_id] =
231  "Preview INTENSITY");
232  m_win_int[sensor_id]->setPos(330, 5 + 250 * sensor_id);
233  }
234  m_win_int[sensor_id]->showImage(
235  out_obs.intensityImages[sensor_id]);
236  }
237  }
238  }
239  else
240  {
241  if (m_win_range[sensor_id]) m_win_range[sensor_id].reset();
242  if (m_win_int[sensor_id]) m_win_int[sensor_id].reset();
243  }
244  }
245  cout << "getNextObservation took " << 1000 * tictac.Tac() << "ms\n";
246 #else
247  THROW_EXCEPTION("MRPT was built without OpenNI2 support");
248 #endif // MRPT_HAS_OPENNI2
249 }
250 
251 /* -----------------------------------------------------
252 setPathForExternalImages
253 ----------------------------------------------------- */
255  [maybe_unused]] const std::string& directory)
256 {
257  // Ignore for now. It seems performance is better grabbing everything
258  // to a single big file than creating hundreds of smaller files per
259  // second...
260  return;
261 
262  // if (!mrpt::system::createDirectory( directory ))
263  // {
264  // THROW_EXCEPTION_FMT("Error: Cannot create the directory for
265  // externally
266  // saved images: %s",directory.c_str() )
267  // }
268  // m_path_for_external_images = directory;
269 }
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:87
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"...
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.
constexpr double DEG2RAD(const double x)
Degrees to radians.
This namespace contains representation of robot actions and observations.
mrpt::math::CMatrix_u16 rangeImages[NUM_SENSORS]
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters) ...
void setFromMatrix(const MAT &m, bool matrix_is_normalized=true)
Set the image from a matrix, interpreted as grayscale intensity values, in the range [0...
Definition: img/CImage.h:844
#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
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:76
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)
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:148
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)...



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020