| 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 |
} |