COpenNI2_RGBD360.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */

#include "hwdrivers-precomp.h"  // Precompiled header

#include <mrpt/hwdrivers/COpenNI2_RGBD360.h>
#include <mrpt/obs/CObservationRGBD360.h>
#include <mrpt/utils/CTimeLogger.h>

// Universal include for all versions of OpenCV
#include <mrpt/otherlibs/do_opencv_includes.h>

using namespace mrpt::hwdrivers;
using namespace mrpt::system;
using namespace mrpt::obs;
using namespace mrpt::math;
using namespace std;
using mrpt::utils::CTicTac;
using mrpt::utils::DEG2RAD;
using mrpt::obs::CObservationRGBD360;

IMPLEMENTS_GENERIC_SENSOR(COpenNI2_RGBD360, mrpt::hwdrivers)

/*-------------------------------------------------------------
ctor
-------------------------------------------------------------*/
COpenNI2_RGBD360::COpenNI2_RGBD360()
        : m_sensorPoseOnRobot(),
          m_preview_window(false),
          m_preview_window_decimation(1),
          m_preview_decim_counter_range(0),
          m_preview_decim_counter_rgb(0),

          m_grab_rgb(true),
          m_grab_depth(true),
          m_grab_3D_points(true)
{
        // Default label:
        m_sensorLabel = "RGBD360";
}

/*-------------------------------------------------------------
dtor
-------------------------------------------------------------*/
COpenNI2_RGBD360::~COpenNI2_RGBD360()
{
#if MRPT_HAS_OPENNI2
        kill();
#endif  // MRPT_HAS_OPENNI2
}

/** This method can or cannot be implemented in the derived class, depending on
* the need for it.
*  \exception This method must throw an exception with a descriptive message if
* some critical error is found.
*/
void COpenNI2_RGBD360::initialize()
{
#if MRPT_HAS_OPENNI2
        // Check and list the available devices
        getConnectedDevices();

        if (getNumDevices() < NUM_SENSORS)
        {
                cout << "Num required sensors " << NUM_SENSORS << endl;
                cout << "Not enough devices connected -> EXIT\n";
                return;
        }
        cout << "COpenNI2_RGBD360 initializes correctly.\n";

        for (unsigned sensor_id = 0; sensor_id < (unsigned int)NUM_SENSORS;
                 sensor_id++)
                open(sensor_id);
#else
        THROW_EXCEPTION("MRPT was built without OpenNI2 support")
#endif  // MRPT_HAS_OPENNI2
}

/** This method will be invoked at a minimum rate of "process_rate" (Hz)
*  \exception This method must throw an exception with a descriptive message if
* some critical error is found.
*/
void COpenNI2_RGBD360::doProcess()
{
#if MRPT_HAS_OPENNI2
        cout << "COpenNI2_RGBD360::doProcess...\n";

        bool thereIs, hwError;

        CObservationRGBD360::Ptr newObs =
                mrpt::make_aligned_shared<CObservationRGBD360>();
        //      CObservation3DRangeScan::Ptr newObs =
        // mrpt::make_aligned_shared<CObservation3DRangeScan>();

        assert(getNumDevices() > 0);
        //  unsigned sensor_id = COpenNI2Generic::vOpenDevices.front();
        getNextObservation(*newObs, thereIs, hwError);

        if (hwError)
        {
                m_state = ssError;
                THROW_EXCEPTION("Couldn't communicate to the OpenNI2 sensor!");
        }

        if (thereIs)
        {
                m_state = ssWorking;

                std::vector<mrpt::utils::CSerializable::Ptr> objs;
                if (m_grab_rgb || m_grab_depth || m_grab_3D_points)
                        objs.push_back(newObs);

                appendObservations(objs);
        }
#else
        THROW_EXCEPTION("MRPT was built without OpenNI2 support")
#endif  // MRPT_HAS_OPENNI2
}

/** 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)
*  \exception This method must throw an exception with a descriptive message if
* some critical parameter is missing or has an invalid value.
*/
void COpenNI2_RGBD360::loadConfig_sensorSpecific(
        const mrpt::utils::CConfigFileBase& configSource,
        const std::string& iniSection)
{
        cout << "COpenNI2_RGBD360::loadConfig_sensorSpecific...\n";

        m_sensorPoseOnRobot.setFromValues(
                configSource.read_float(iniSection, "pose_x", 0),
                configSource.read_float(iniSection, "pose_y", 0),
                configSource.read_float(iniSection, "pose_z", 0),
                DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
                DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
                DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));

        m_preview_window =
                configSource.read_bool(iniSection, "preview_window", m_preview_window);

        m_width = configSource.read_int(iniSection, "width", 0);
        m_height = configSource.read_int(iniSection, "height", 0);
        m_fps = configSource.read_float(iniSection, "fps", 0);
        std::cout << "width " << m_width << " height " << m_height << " fps "
                          << m_fps << endl;

        m_grab_rgb = configSource.read_bool(iniSection, "grab_image", m_grab_rgb);
        m_grab_depth =
                configSource.read_bool(iniSection, "grab_depth", m_grab_depth);
        m_grab_3D_points =
                configSource.read_bool(iniSection, "grab_3D_points", m_grab_3D_points);

        //  m_num_sensors = configSource.read_int(iniSection,"m_num_sensors",0);
}

/** The main data retrieving function, to be called after calling loadConfig()
* and initialize().
*  \param out_obs The output retrieved observation (only if there_is_obs=true).
*  \param there_is_obs If set to false, there was no new observation.
*  \param hardware_error True on hardware/comms error.
*
* \sa doProcess
*/
void COpenNI2_RGBD360::getNextObservation(
        CObservationRGBD360& out_obs, bool& there_is_obs, bool& hardware_error)
{
#if MRPT_HAS_OPENNI2
        CTicTac tictac;
        tictac.Tic();
        //      cout << "COpenNI2_RGBD360::getNextObservation \n";

        there_is_obs = false;
        hardware_error = false;

        CObservationRGBD360 newObs;
        // Set intensity image ----------------------
        if (m_grab_rgb) newObs.hasIntensityImage = true;
        // Set range image --------------------------
        if (m_grab_depth || m_grab_3D_points) newObs.hasRangeImage = true;

        newObs.timestamp = mrpt::system::getCurrentTime();

        for (unsigned sensor_id = 0; sensor_id < (unsigned int)NUM_SENSORS;
                 sensor_id++)
        {
                //              cout << "Get sensor " << sensor_id << " \n";
                bool there_is_obs, hardware_error;
                getNextFrameRGB(
                        newObs.intensityImages[sensor_id], newObs.timestamps[sensor_id],
                        there_is_obs, hardware_error, sensor_id);
                getNextFrameD(
                        newObs.rangeImages[sensor_id], newObs.timestamps[sensor_id],
                        there_is_obs, hardware_error, sensor_id);
        }

        // preview in real-time?
        for (unsigned sensor_id = 0; sensor_id < (unsigned int)NUM_SENSORS;
                 sensor_id++)
        {
                if (m_preview_window)
                {
                        if (out_obs.hasRangeImage)
                        {
                                if (++m_preview_decim_counter_range >
                                        m_preview_window_decimation)
                                {
                                        m_preview_decim_counter_range = 0;
                                        if (!m_win_range[sensor_id])
                                        {
                                                m_win_range[sensor_id] = mrpt::make_aligned_shared<
                                                        mrpt::gui::CDisplayWindow>("Preview RANGE");
                                                m_win_range[sensor_id]->setPos(5, 5 + 250 * sensor_id);
                                        }

                                        // Normalize the image
                                        mrpt::utils::CImage img;
                                        img.setFromMatrix(out_obs.rangeImages[sensor_id]);
                                        CMatrixFloat r = out_obs.rangeImages[sensor_id] *
                                                                         float(1.0 / this->m_maxRange);
                                        m_win_range[sensor_id]->showImage(img);
                                }
                        }
                        if (out_obs.hasIntensityImage)
                        {
                                if (++m_preview_decim_counter_rgb > m_preview_window_decimation)
                                {
                                        m_preview_decim_counter_rgb = 0;
                                        if (!m_win_int[sensor_id])
                                        {
                                                m_win_int[sensor_id] = mrpt::make_aligned_shared<
                                                        mrpt::gui::CDisplayWindow>("Preview INTENSITY");
                                                m_win_int[sensor_id]->setPos(330, 5 + 250 * sensor_id);
                                        }
                                        m_win_int[sensor_id]->showImage(
                                                out_obs.intensityImages[sensor_id]);
                                }
                        }
                }
                else
                {
                        if (m_win_range[sensor_id]) m_win_range[sensor_id].reset();
                        if (m_win_int[sensor_id]) m_win_int[sensor_id].reset();
                }
        }
        cout << "getNextObservation took " << 1000 * tictac.Tac() << "ms\n";
#else
        MRPT_UNUSED_PARAM(out_obs);
        MRPT_UNUSED_PARAM(there_is_obs);
        MRPT_UNUSED_PARAM(hardware_error);
        MRPT_UNUSED_PARAM(out_obs);
        THROW_EXCEPTION("MRPT was built without OpenNI2 support")
#endif  // MRPT_HAS_OPENNI2
}

/* -----------------------------------------------------
setPathForExternalImages
----------------------------------------------------- */
void COpenNI2_RGBD360::setPathForExternalImages(const std::string& directory)
{
        MRPT_UNUSED_PARAM(directory);
        // Ignore for now. It seems performance is better grabbing everything
        // to a single big file than creating hundreds of smaller files per
        // second...
        return;

        //      if (!mrpt::system::createDirectory( directory ))
        //      {
        //              THROW_EXCEPTION_FMT("Error: Cannot create the directory for
        // externally
        // saved images: %s",directory.c_str() )
        //      }
        //      m_path_for_external_images = directory;
}