C2DRangeFinderAbstract.h

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 |
   +------------------------------------------------------------------------+ */
#ifndef C2DRangeFinderAbstract_H
#define C2DRangeFinderAbstract_H

#include <mrpt/utils/CStream.h>
#include <mrpt/utils/COutputLogger.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/utils/CConfigFileBase.h>
#include <mrpt/hwdrivers/CGenericSensor.h>
#include <mrpt/math/CPolygon.h>
#include <mrpt/gui/CDisplayWindow3D.h>

namespace mrpt
{
namespace hwdrivers
{
/** This is the base, abstract class for "software drivers" interfaces to 2D
 * scanners (laser range finders).
  *  Physical devices may be interfaced through a serial port, a USB
 * connection,etc. but this class
  *   abstract those details throught the "binding" of the specific scanner
 * driver to a given I/O channel,
  *   which must be set by calling "hwdrivers::C2DRangeFinderAbstract::bindIO".
 * See also the derived classes.
  *
  *  There is support for "exclusion polygons", areas where points, if detected,
 * should be marked as invalid.
  *  Those areas are useful in cases where the scanner always detects part of
 * the vehicle itself, and those
  *   points want to be ignored (see
 * C2DRangeFinderAbstract::loadExclusionAreas).
  *
  * \sa comms::CSerialPort
  * \ingroup mrpt_hwdrivers_grp
  */
class C2DRangeFinderAbstract : public mrpt::utils::COutputLogger,
                                                           public mrpt::hwdrivers::CGenericSensor
{
   private:
        mrpt::obs::CObservation2DRangeScan m_lastObservation;
        bool m_lastObservationIsNew;
        bool m_hardwareError;

        /** For being thread-safe.
          */
        std::mutex m_csChangeStream, m_csLastObservation;

        /** A dynamic object used as buffer in doProcess */
        mrpt::obs::CObservation2DRangeScan::Ptr m_nextObservation;

        /** A list of optional exclusion polygons, in coordinates relative to the
         * vehicle, that is, taking into account the "sensorPose". */
        mrpt::obs::CObservation2DRangeScan::TListExclusionAreasWithRanges
                m_lstExclusionPolys;
        /** A list of pairs of angles <init,end> such as all sensor ranges falling
         * in those forbiden angles will be marked as invalid. */
        std::vector<std::pair<double, double>> m_lstExclusionAngles;

        /** If true, shows a 3D window with a preview of the grabber data */
        bool m_showPreview;
        mrpt::gui::CDisplayWindow3D::Ptr m_win;

   protected:
        /** The I/O channel (will be nullptr if not bound). */
        utils::CStream* m_stream;

        /** Should be call by derived classes at "loadConfig" (loads exclusion areas
          *AND exclusion angles).
          *  This loads a sequence of vertices of a polygon given by its (x,y)
          *coordinates relative to the vehicle, that is, taking into account the
          *"sensorPose".
          *   - exclusionZone%u_x
          *   - exclusionZone%u_y
          *   for %u=1,2,3,...
          *   All points within the 2D polygon will be ignored, for any Z, unless an
          *optional entry is found:
          *   - exclusionZone%u_z=[z_min z_max]
          *       In that case, only the points within the 2D polygon AND the given
          *range in Z will be ignored.
          *
          *  The number of zones is variable, but they must start at 1 and be
          *consecutive.
          *
          * This also loads any other common params (e.g. 'preview')
          * \sa filterByExclusionAreas
          */
        void loadCommonParams(
                const mrpt::utils::CConfigFileBase& configSource,
                const std::string& iniSection);

        /** Mark as invalid those points which (x,y) coordinates fall within the
         * exclusion polygons.
          * \sa loadExclusionAreas
          */
        void filterByExclusionAreas(mrpt::obs::CObservation2DRangeScan& obs) const;

        /** Mark as invalid those ranges in a set of forbiden angle ranges.
          * \sa loadExclusionAreas
          */
        void filterByExclusionAngles(mrpt::obs::CObservation2DRangeScan& obs) const;

        /** Must be called inside the capture method to allow optional GUI preview
         * of scans */
        void processPreview(const mrpt::obs::CObservation2DRangeScan& obs);

   public:
        /** Default constructor */
        C2DRangeFinderAbstract();
        /** Destructor */
        virtual ~C2DRangeFinderAbstract();

        /** Enables GUI visualization in real-time */
        void showPreview(bool enable = true) { m_showPreview = enable; }
        /** Binds the object to a given I/O channel.
          *  The stream object must not be deleted before the destruction of this
         * class.
          * \sa comms::CSerialPort
          */
        void bindIO(mrpt::utils::CStream* streamIO);

        /** Get the last observation from the sensor, if available, and unmarks it
         * as being "the last one" (thus a new scan must arrive or subsequent calls
         * will find no new observations).
          */
        void getObservation(
                bool& outThereIsObservation,
                mrpt::obs::CObservation2DRangeScan& outObservation,
                bool& hardwareError);

        /** Main method for a CGenericSensor */
        void doProcess();

        /** Specific laser scanner "software drivers" must process here new data
         * from the I/O stream, and, if a whole scan has arrived, return it.
          *  This method MUST BE CALLED in a timely fashion by the user to allow the
         * proccessing of incoming data. It can be run in a different thread safely.
          */
        virtual void doProcessSimple(
                bool& outThereIsObservation,
                mrpt::obs::CObservation2DRangeScan& outObservation,
                bool& hardwareError) = 0;

        /** Enables the scanning mode (which may depend on the specific laser
         * device); this must be called before asking for observations to assure
         * that the protocol has been initializated.
          * \return If everything works "true", or "false" if there is any error.
          */
        virtual bool turnOn() = 0;

        /** Disables the scanning mode (this can be used to turn the device in low
         * energy mode, if available)
          * \return If everything works "true", or "false" if there is any error.
          */
        virtual bool turnOff() = 0;

};  // End of class
}  // End of namespace
}  // End of namespace

#endif