Debian Codegraph @

libs/hwdrivers/src/COpenNI2_RGBD360.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/COpenNI2_RGBD360.h>
13 #include <mrpt/obs/CObservationRGBD360.h>
14 #include <mrpt/system/CTimeLogger.h>
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;
25 using mrpt::obs::CObservationRGBD360;
26 using mrpt::system::CTicTac;
27
28 IMPLEMENTS_GENERIC_SENSOR(COpenNI2_RGBD360, mrpt::hwdrivers)
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 -------------------------------------------------------------*/
43 COpenNI2_RGBD360::~COpenNI2_RGBD360()
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 */
55 void COpenNI2_RGBD360::initialize()
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; sensor_id++) open(sensor_id);
70 #else
71 THROW_EXCEPTION("MRPT was built without OpenNI2 support");
72 #endif // MRPT_HAS_OPENNI2
73 }
74
75 /** This method will be invoked at a minimum rate of "process_rate" (Hz)
76 * \exception This method must throw an exception with a descriptive message if
77 * some critical error is found.
78 */
79 void COpenNI2_RGBD360::doProcess()
80 {
81 #if MRPT_HAS_OPENNI2
82 cout << "COpenNI2_RGBD360::doProcess...\n";
83
84 bool thereIs, hwError;
85
86 CObservationRGBD360::Ptr newObs = std::make_shared<CObservationRGBD360>();
87 // CObservation3DRangeScan::Ptr newObs =
88 // std::make_shared<CObservation3DRangeScan>();
89
90 assert(getNumDevices() > 0);
91 // unsigned sensor_id = COpenNI2Generic::vOpenDevices.front();
92 getNextObservation(*newObs, thereIs, hwError);
93
94 if (hwError)
95 {
96 m_state = ssError;
97 THROW_EXCEPTION("Couldn't communicate to the OpenNI2 sensor!");
98 }
99
100 if (thereIs)
101 {
102 m_state = ssWorking;
103
104 std::vector<mrpt::serialization::CSerializable::Ptr> objs;
105 if (m_grab_rgb || m_grab_depth || m_grab_3D_points) objs.push_back(newObs);
106
107 appendObservations(objs);
108 }
109 #else
110 THROW_EXCEPTION("MRPT was built without OpenNI2 support");
111 #endif // MRPT_HAS_OPENNI2
112 }
113
114 /** Loads specific configuration for the device from a given source of
115 * configuration parameters, for example, an ".ini" file, loading from the
116 * section "[iniSection]" (see config::CConfigFileBase and derived classes)
117 * \exception This method must throw an exception with a descriptive message if
118 * some critical parameter is missing or has an invalid value.
119 */
120 void COpenNI2_RGBD360::loadConfig_sensorSpecific(
121 const mrpt::config::CConfigFileBase& configSource, const std::string& iniSection)
122 {
123 cout << "COpenNI2_RGBD360::loadConfig_sensorSpecific...\n";
124
125 m_sensorPoseOnRobot.setFromValues(
126 configSource.read_float(iniSection, "pose_x", 0),
127 configSource.read_float(iniSection, "pose_y", 0),
128 configSource.read_float(iniSection, "pose_z", 0),
129 DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
130 DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
131 DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));
132
133 m_preview_window = configSource.read_bool(iniSection, "preview_window", m_preview_window);
134
135 m_width = configSource.read_int(iniSection, "width", 0);
136 m_height = configSource.read_int(iniSection, "height", 0);
137 m_fps = configSource.read_float(iniSection, "fps", 0);
138 std::cout << "width " << m_width << " height " << m_height << " fps " << m_fps << endl;
139
140 m_grab_rgb = configSource.read_bool(iniSection, "grab_image", m_grab_rgb);
141 m_grab_depth = configSource.read_bool(iniSection, "grab_depth", m_grab_depth);
142 m_grab_3D_points = configSource.read_bool(iniSection, "grab_3D_points", m_grab_3D_points);
143
144 // m_num_sensors = configSource.read_int(iniSection,"m_num_sensors",0);
145 }
146
147 /** The main data retrieving function, to be called after calling loadConfig()
148 * and initialize().
149 * \param out_obs The output retrieved observation (only if there_is_obs=true).
150 * \param there_is_obs If set to false, there was no new observation.
151 * \param hardware_error True on hardware/comms error.
152 *
153 * \sa doProcess
154 */
155 void COpenNI2_RGBD360::getNextObservation(
156 [[maybe_unused]] CObservationRGBD360& out_obs,
157 [[maybe_unused]] bool& there_is_obs,
158 [[maybe_unused]] bool& hardware_error)
159 {
160 #if MRPT_HAS_OPENNI2
161 CTicTac tictac;
162 tictac.Tic();
163 // cout << "COpenNI2_RGBD360::getNextObservation \n";
164
165 there_is_obs = false;
166 hardware_error = false;
167
168 CObservationRGBD360 newObs;
169 // Set intensity image ----------------------
170 if (m_grab_rgb) newObs.hasIntensityImage = true;
171 // Set range image --------------------------
172 if (m_grab_depth || m_grab_3D_points) newObs.hasRangeImage = true;
173
174 newObs.timestamp = mrpt::Clock::now();
175
176 for (unsigned sensor_id = 0; sensor_id < (unsigned int)NUM_SENSORS; sensor_id++)
177 {
178 getNextFrameRGB(
179 newObs.intensityImages[sensor_id], newObs.timestamps[sensor_id], there_is_obs,
180 hardware_error, sensor_id);
181 getNextFrameD(
182 newObs.rangeImages[sensor_id], newObs.timestamps[sensor_id], there_is_obs, hardware_error,
183 sensor_id);
184 }
185
186 // preview in real-time?
187 for (unsigned sensor_id = 0; sensor_id < (unsigned int)NUM_SENSORS; sensor_id++)
188 {
189 if (m_preview_window)
190 {
191 if (out_obs.hasRangeImage)
192 {
193 if (++m_preview_decim_counter_range > m_preview_window_decimation)
194 {
195 m_preview_decim_counter_range = 0;
196 if (!m_win_range[sensor_id])
197 {
198 m_win_range[sensor_id] = mrpt::gui::CDisplayWindow::Create("Preview RANGE");
199 m_win_range[sensor_id]->setPos(5, 5 + 250 * sensor_id);
200 }
201
202 // Normalize the image
203 mrpt::img::CImage img;
204 const Eigen::MatrixXf r = out_obs.rangeImages[sensor_id].asEigen().cast<float>() *
205 out_obs.rangeUnits * float(1.0 / this->m_maxRange);
206 img.setFromMatrix(r, true /*normalized 0-1 */);
207 m_win_range[sensor_id]->showImage(img);
208 }
209 }
210 if (out_obs.hasIntensityImage)
211 {
212 if (++m_preview_decim_counter_rgb > m_preview_window_decimation)
213 {
214 m_preview_decim_counter_rgb = 0;
215 if (!m_win_int[sensor_id])
216 {
217 m_win_int[sensor_id] = mrpt::gui::CDisplayWindow::Create("Preview INTENSITY");
218 m_win_int[sensor_id]->setPos(330, 5 + 250 * sensor_id);
219 }
220 m_win_int[sensor_id]->showImage(out_obs.intensityImages[sensor_id]);
221 }
222 }
223 }
224 else
225 {
226 if (m_win_range[sensor_id]) m_win_range[sensor_id].reset();
227 if (m_win_int[sensor_id]) m_win_int[sensor_id].reset();
228 }
229 }
230 cout << "getNextObservation took " << 1000 * tictac.Tac() << "ms\n";
231 #else
232 THROW_EXCEPTION("MRPT was built without OpenNI2 support");
233 #endif // MRPT_HAS_OPENNI2
234 }
235
236 /* -----------------------------------------------------
237 setPathForExternalImages
238 ----------------------------------------------------- */
239 void COpenNI2_RGBD360::setPathForExternalImages([[maybe_unused]] const std::string& directory)
240 {
241 // Ignore for now. It seems performance is better grabbing everything
242 // to a single big file than creating hundreds of smaller files per
243 // second...
244 return;
245
246 // if (!mrpt::system::createDirectory( directory ))
247 // {
248 // THROW_EXCEPTION_FMT("Error: Cannot create the directory for
249 // externally
250 // saved images: %s",directory.c_str() )
251 // }
252 // m_path_for_external_images = directory;
253 }