C2DRangeFinderAbstract.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/config/CConfigFileBase.h>
#include <mrpt/gui/CDisplayWindow3D.h>
#include <mrpt/hwdrivers/CGenericSensor.h>
#include <mrpt/math/CPolygon.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/system/COutputLogger.h>

namespace mrpt::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::system::COutputLogger,
                               public mrpt::hwdrivers::CGenericSensor
{
   private:
    mrpt::obs::CObservation2DRangeScan m_lastObservation;
    bool m_lastObservationIsNew{false};
    bool m_hardwareError{false};

    /** 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{false};
    mrpt::gui::CDisplayWindow3D::Ptr m_win;

   protected:
    /** The I/O channel (will be nullptr if not bound). */
    std::shared_ptr<mrpt::io::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::config::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 */
    ~C2DRangeFinderAbstract() override;

    /** 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(const std::shared_ptr<mrpt::io::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() override;

    /** 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;

    /** Returns the empirical, filtered estimation for the period at which whole
     * scans are being returned from calls to doProcessSimple()
     * \note:  Units: seconds */
    double getEstimatedScanPeriod() const { return m_estimated_scan_period; }

   protected:
    /** Must be called from doProcessSimple() implementations */
    void internal_notifyGoodScanNow();

    /** Must be called from doProcessSimple() implementations.
     * Returns true if ok, false if this seems strange and should
     * return an error condition to the user. */
    bool internal_notifyNoScanReceived();

   private:
    /// Used in internal_notifyGoodScanNow()
    mrpt::system::TTimeStamp m_last_good_scan{INVALID_TIMESTAMP};
    /// Updated in internal_notifyGoodScanNow()
    double m_estimated_scan_period{1.0};
    /// Used in internal_notifyNoScanReceived()
    int m_failure_waiting_scan_counter{0}, m_max_missed_scan_failures{10};

};  // End of class
}  // namespace mrpt::hwdrivers