Debian Codegraph @

libs/hwdrivers/src/COpenNI2Sensor.cpp

raw
1 /* +------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | https://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2026, 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 //
12 #include <mrpt/hwdrivers/COpenNI2Generic.h>
13 #include <mrpt/hwdrivers/COpenNI2Sensor.h>
14 #include <mrpt/img/TStereoCamera.h>
15 #include <mrpt/obs/CObservation3DRangeScan.h>
16 #include <mrpt/poses/CPose3DQuat.h>
17 #include <mrpt/system/CTimeLogger.h>
18
19 // Universal include for all versions of OpenCV
20 #include <mrpt/3rdparty/do_opencv_includes.h>
21
22 using namespace mrpt::hwdrivers;
23 using namespace mrpt::system;
24 using namespace mrpt::obs;
25 using namespace mrpt::math;
26 using namespace std;
27 using mrpt::DEG2RAD;
28
29 bool isValidParameter(const mrpt::img::TCamera& param)
30 {
31 return param.ncols > 0 && param.nrows > 0;
32 }
33
34 IMPLEMENTS_GENERIC_SENSOR(COpenNI2Sensor, mrpt::hwdrivers)
35
36 /*-------------------------------------------------------------
37 ctor
38 -------------------------------------------------------------*/
39 COpenNI2Sensor::COpenNI2Sensor() :
40 m_sensorPoseOnRobot(),
41
42 m_relativePoseIntensityWRTDepth(0, 0, 0, -90.0_deg, 0.0_deg, -90.0_deg)
43
44 {
45 // Default label:
46 m_sensorLabel = "OPENNI2";
47
48 // =========== Default params ===========
49 // ----- RGB -----
50 m_cameraParamsRGB.ncols = 0;
51 m_cameraParamsRGB.nrows = 0;
52
53 m_cameraParamsRGB.cx(-1);
54 m_cameraParamsRGB.cy(-1);
55 m_cameraParamsRGB.fx(-1);
56 m_cameraParamsRGB.fy(-1);
57
58 m_cameraParamsRGB.dist.fill(0);
59
60 // ----- Depth -----
61 m_cameraParamsDepth.ncols = 0;
62 m_cameraParamsDepth.nrows = 0;
63
64 m_cameraParamsDepth.cx(-1);
65 m_cameraParamsDepth.cy(-1);
66 m_cameraParamsDepth.fx(-1);
67 m_cameraParamsDepth.fy(-1);
68
69 m_cameraParamsDepth.dist.fill(0);
70 }
71
72 /*-------------------------------------------------------------
73 dtor
74 -------------------------------------------------------------*/
75 COpenNI2Sensor::~COpenNI2Sensor()
76 {
77 #if MRPT_HAS_OPENNI2
78 close(m_user_device_number);
79 #endif // MRPT_HAS_OPENNI2
80 }
81
82 /** This method can or cannot be implemented in the derived class, depending on
83 * the need for it.
84 * \exception This method must throw an exception with a descriptive message if
85 * some critical error is found.
86 */
87 void COpenNI2Sensor::initialize()
88 {
89 #if MRPT_HAS_OPENNI2
90 try
91 {
92 if (getConnectedDevices() <= 0)
93 { // Check and list the available devices. If there is at least one
94 // device connected, open the first in the list.
95 return;
96 }
97 {
98 if (m_serial_number != 0)
99 {
100 openDeviceBySerial(m_serial_number);
101 if (getDeviceIDFromSerialNum(m_serial_number, m_user_device_number) == false)
102 {
103 THROW_EXCEPTION(
104 mrpt::format("Failed to find sensor_id from serial number(%d).", m_serial_number));
105 }
106 }
107 else
108 open(m_user_device_number);
109 }
110 if (isOpen(m_user_device_number) == false)
111 {
112 THROW_EXCEPTION(mrpt::format("Failed to open OpenNI2 device(%d).", m_user_device_number));
113 }
114 /* If camera parameter is not read from ini file, we get the parameters
115 * from OpenNI2. */
116 if (isValidParameter(m_cameraParamsDepth) == false)
117 {
118 if (getDepthSensorParam(m_cameraParamsDepth, m_user_device_number) == false)
119 {
120 THROW_EXCEPTION("Failed to get Depth camera parameters.");
121 }
122 }
123 if (isValidParameter(m_cameraParamsRGB) == false)
124 {
125 if (getColorSensorParam(m_cameraParamsRGB, m_user_device_number) == false)
126 {
127 THROW_EXCEPTION("Failed to get RGB camera parameters.");
128 }
129 }
130 }
131 catch (std::logic_error& e)
132 {
133 throw(e);
134 }
135 #else
136 THROW_EXCEPTION("MRPT was built without OpenNI2 support");
137 #endif // MRPT_HAS_OPENNI2
138 }
139
140 /** This method will be invoked at a minimum rate of "process_rate" (Hz)
141 * \exception This method must throw an exception with a descriptive message if
142 * some critical error is found.
143 */
144 void COpenNI2Sensor::doProcess()
145 {
146 #if MRPT_HAS_OPENNI2
147 // cout << "COpenNI2Sensor::doProcess...\n";
148
149 bool thereIs, hwError;
150
151 CObservation3DRangeScan::Ptr newObs = std::make_shared<CObservation3DRangeScan>();
152
153 assert(getNumDevices() > 0);
154 getNextObservation(*newObs, thereIs, hwError);
155
156 if (hwError)
157 {
158 m_state = ssError;
159 THROW_EXCEPTION("Couldn't communicate to the OpenNI2 sensor!");
160 }
161
162 if (thereIs)
163 {
164 m_state = ssWorking;
165
166 std::vector<mrpt::serialization::CSerializable::Ptr> objs;
167 if (m_grab_image || m_grab_depth || m_grab_3D_points) objs.push_back(newObs);
168
169 appendObservations(objs);
170 }
171 #else
172 THROW_EXCEPTION("MRPT was built without OpenNI2 support");
173 #endif // MRPT_HAS_OPENNI2
174 }
175
176 /** Loads specific configuration for the device from a given source of
177 * configuration parameters, for example, an ".ini" file, loading from the
178 * section "[iniSection]" (see config::CConfigFileBase and derived classes)
179 * \exception This method must throw an exception with a descriptive message if
180 * some critical parameter is missing or has an invalid value.
181 */
182 void COpenNI2Sensor::loadConfig_sensorSpecific(
183 const mrpt::config::CConfigFileBase& configSource, const std::string& iniSection)
184 {
185 cout << "COpenNI2Sensor::loadConfig_sensorSpecific...\n";
186
187 m_sensorPoseOnRobot.setFromValues(
188 configSource.read_float(iniSection, "pose_x", 0),
189 configSource.read_float(iniSection, "pose_y", 0),
190 configSource.read_float(iniSection, "pose_z", 0),
191 DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
192 DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
193 DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));
194
195 m_preview_window = configSource.read_bool(iniSection, "preview_window", m_preview_window);
196
197 m_width = configSource.read_int(iniSection, "width", 0);
198 m_height = configSource.read_int(iniSection, "height", 0);
199 m_fps = configSource.read_float(iniSection, "fps", 0);
200 std::cout << "width " << m_width << " height " << m_height << " fps " << m_fps << endl;
201
202 bool hasRightCameraSection = configSource.sectionExists(iniSection + string("_RIGHT"));
203 bool hasLeftCameraSection = configSource.sectionExists(iniSection + string("_LEFT"));
204 bool hasLeft2RightPose = configSource.sectionExists(iniSection + string("_LEFT2RIGHT_POSE"));
205
206 mrpt::img::TStereoCamera sc;
207
208 try
209 {
210 sc.loadFromConfigFile(iniSection, configSource);
211 }
212 catch (const std::exception& e)
213 {
214 std::cout << "[COpenNI2Sensor::loadConfig_sensorSpecific] Warning: "
215 "Ignoring error loading calibration parameters:\n"
216 << e.what();
217 }
218 if (hasRightCameraSection)
219 {
220 m_cameraParamsRGB = sc.rightCamera;
221 }
222 if (hasLeftCameraSection)
223 {
224 m_cameraParamsDepth = sc.leftCamera;
225 }
226 if (hasLeft2RightPose)
227 {
228 const mrpt::poses::CPose3D twist(0, 0, 0, -90.0_deg, 0.0_deg, -90.0_deg);
229 m_relativePoseIntensityWRTDepth =
230 twist + mrpt::poses::CPose3D(mrpt::poses::CPose3DQuat(sc.rightCameraPose));
231 }
232
233 // Id:
234 m_user_device_number = configSource.read_int(iniSection, "device_number", m_user_device_number);
235 // cout << "LOAD m_user_device_number " << m_user_device_number << endl;
236 m_serial_number = configSource.read_int(iniSection, "serial_number", m_serial_number);
237
238 m_grab_image = configSource.read_bool(iniSection, "grab_image", m_grab_image);
239 m_grab_depth = configSource.read_bool(iniSection, "grab_depth", m_grab_depth);
240 m_grab_3D_points = configSource.read_bool(iniSection, "grab_3D_points", m_grab_3D_points);
241
242 {
243 std::string s = configSource.read_string(iniSection, "relativePoseIntensityWRTDepth", "");
244 if (!s.empty()) m_relativePoseIntensityWRTDepth.fromString(s);
245 }
246 }
247
248 /** The main data retrieving function, to be called after calling loadConfig()
249 * and initialize().
250 * \param out_obs The output retrieved observation (only if there_is_obs=true).
251 * \param there_is_obs If set to false, there was no new observation.
252 * \param hardware_error True on hardware/comms error.
253 *
254 * \sa doProcess
255 */
256 void COpenNI2Sensor::getNextObservation(
257 [[maybe_unused]] mrpt::obs::CObservation3DRangeScan& out_obs,
258 [[maybe_unused]] bool& there_is_obs,
259 [[maybe_unused]] bool& hardware_error)
260 {
261 #if MRPT_HAS_OPENNI2
262 // cout << "COpenNI2Sensor::getNextObservation \n";
263
264 // Read a frame (depth + rgb)
265 getNextFrameRGBD(out_obs, there_is_obs, hardware_error, m_user_device_number);
266
267 // Set common data into observation:
268 // --------------------------------------
269 out_obs.sensorLabel = m_sensorLabel;
270 out_obs.sensorPose = m_sensorPoseOnRobot;
271 out_obs.relativePoseIntensityWRTDepth = m_relativePoseIntensityWRTDepth;
272
273 out_obs.cameraParams = m_cameraParamsDepth;
274 out_obs.cameraParamsIntensity = m_cameraParamsRGB;
275
276 // 3D point cloud:
277 if (out_obs.hasRangeImage && m_grab_3D_points)
278 {
279 out_obs.unprojectInto(out_obs);
280
281 if (!m_grab_depth)
282 {
283 out_obs.hasRangeImage = false;
284 out_obs.rangeImage.resize(0, 0);
285 }
286 }
287
288 // preview in real-time?
289 if (m_preview_window)
290 {
291 if (out_obs.hasRangeImage)
292 {
293 if (++m_preview_decim_counter_range > m_preview_window_decimation)
294 {
295 m_preview_decim_counter_range = 0;
296 if (!m_win_range)
297 {
298 m_win_range = mrpt::gui::CDisplayWindow::Create("Preview RANGE");
299 m_win_range->setPos(5, 5);
300 }
301
302 m_win_range->showImage(out_obs.rangeImage_getAsImage());
303 }
304 }
305 if (out_obs.hasIntensityImage)
306 {
307 if (++m_preview_decim_counter_rgb > m_preview_window_decimation)
308 {
309 m_preview_decim_counter_rgb = 0;
310 if (!m_win_int)
311 {
312 m_win_int = mrpt::gui::CDisplayWindow::Create("Preview INTENSITY");
313 m_win_int->setPos(300, 5);
314 }
315 m_win_int->showImage(out_obs.intensityImage);
316 }
317 }
318 }
319 else
320 {
321 if (m_win_range) m_win_range.reset();
322 if (m_win_int) m_win_int.reset();
323 }
324
325 // cout << "COpenNI2Sensor::getNextObservation finish\n";
326 #else
327 THROW_EXCEPTION("MRPT was built without OpenNI2 support");
328 #endif // MRPT_HAS_OPENNI2
329 }
330
331 /* -----------------------------------------------------
332 setPathForExternalImages
333 ----------------------------------------------------- */
334 void COpenNI2Sensor::setPathForExternalImages([[maybe_unused]] const std::string& directory)
335 {
336 // Ignore for now. It seems performance is better grabbing everything
337 // to a single big file than creating hundreds of smaller files per
338 // second...
339 return;
340
341 // if (!mrpt::system::createDirectory( directory ))
342 // {
343 // THROW_EXCEPTION_FMT("Error: Cannot create the directory for
344 // externally
345 // saved images: %s",directory.c_str() );
346 // }
347 // m_path_for_external_images = directory;
348 }