CHokuyoURG.h

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2015, 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 CHokuyoURG_H
#define CHokuyoURG_H

#include <mrpt/poses/CPose3D.h>
#include <mrpt/hwdrivers/C2DRangeFinderAbstract.h>
#include <mrpt/utils/circular_buffer.h>
#include <mrpt/gui/CDisplayWindow3D.h>

namespace mrpt
{
        namespace hwdrivers
        {
                /** This software driver implements the protocol SCIP-2.0 for interfacing HOKUYO URG, UTM and UXM laser scanners.
                  *  Refer to the wiki page for more details:
                  *    http://www.mrpt.org/Example:HOKUYO_URG/UTM_Laser_Scanner
                  *
                  *  See also the application "rawlog-grabber" for a ready-to-use application to gather data from the scanner.
                  *
                  *  \code
                  *  PARAMETERS IN THE ".INI"-LIKE CONFIGURATION STRINGS:
                  * -------------------------------------------------------
                  *   [supplied_section_name]
                  *    HOKUYO_motorSpeed_rpm=600
                  *    //HOKUYO_HS_mode   = false    // Optional (un-comment line if used): Set/unset the High-sensitivity mode (not on all models/firmwares!)
                  *    COM_port_WIN = COM3
                  *    COM_port_LIN = ttyS0
                  *    pose_x=0.21      // Laser range scaner 3D position in the robot (meters)
                  *    pose_y=0
                  *    pose_z=0.34
                  *    pose_yaw=0       // Angles in degrees
                  *    pose_pitch=0
                  *    pose_roll=0
                  *    //IP_DIR =       192.168.0.10 // Uncommented this and "PORT_DIR" if the used HOKUYO is connected by Ethernet instead of USB
                  *    //PORT_DIR = 10940
                  *
                  *    // Optional: reduced FOV:
                  *    // reduced_fov  = 25 // Deg
                  *
                  *    //preview = true // Enable GUI visualization of captured data
                  *
                  *    // Optional: Exclusion zones to avoid the robot seeing itself:
                  *    //exclusionZone1_x = 0.20 0.30 0.30 0.20
                  *    //exclusionZone1_y = 0.20 0.30 0.30 0.20
                  *
                  *    // Optional: Exclusion zones to avoid the robot seeing itself:
                  *    //exclusionAngles1_ini = 20  // Deg
                  *    //exclusionAngles1_end = 25  // Deg
                  *
                  *  \endcode
                  * \ingroup mrpt_hwdrivers_grp
                  */
                class HWDRIVERS_IMPEXP CHokuyoURG : public C2DRangeFinderAbstract
                {
                        DEFINE_GENERIC_SENSOR(CHokuyoURG)
                public:

                        /** Used in CHokuyoURG::displayVersionInfo */
                        struct TSensorInfo
                        {
                                std::string             model;                  //!< The sensor model
                                double                  d_min,d_max;    //!< Min/Max ranges, in meters.
                                int                             scans_per_360deg;       //!< Number of measuremens per 360 degrees.
                                int                             scan_first,scan_last, scan_front;       //!< First, last, and front step of the scanner angular span.
                                int                             motor_speed_rpm;                //!< Standard motor speed, rpm.
                        };

                private:
                        int             m_firstRange,m_lastRange;   //!< The first and last ranges to consider from the scan.
                        int             m_motorSpeed_rpm;           //!< The motor speed (default=600rpm)
                        poses::CPose3D  m_sensorPose;       //!< The sensor 6D pose:
                        mrpt::utils::circular_buffer<uint8_t> m_rx_buffer; //!< Auxiliary buffer for readings

                        std::string     m_lastSentMeasCmd; //!< The last sent measurement command (MDXXX), including the last 0x0A.

                        bool                    m_verbose;
                        bool                    m_highSensMode;  //!< High sensitivity [HS] mode (default: false)
                        mrpt::gui::CDisplayWindow3DPtr m_win;

                        /** Enables the SCIP2.0 protocol (this must be called at the very begining!).
                          * \return false on any error
                          */
                        bool  enableSCIP20();

                        /** Passes to 115200bps bitrate.
                          * \return false on any error
                          */
                        bool  setHighBaudrate();

                        /** Switchs the laser on.
                          * \return false on any error
                          */
                        bool  switchLaserOn();

                        /** Switchs the laser off
                          * \return false on any error
                          */
                        bool  switchLaserOff();

                        /** Changes the motor speed in rpm's (default 600rpm)
                          * \return false on any error
                          */
                        bool  setMotorSpeed(int motoSpeed_rpm);

                        /** Ask to the device, and print to the debug stream, details about the firmware version,serial number,...
                          * \return false on any error
                          */
                        bool  displayVersionInfo(  );

                        /** Ask to the device, and print to the debug stream, details about the sensor model.
                          *  It also optionally saves all the information in an user supplied data structure "out_data".
                          * \return false on any error
                          */
                        bool  displaySensorInfo( CHokuyoURG::TSensorInfo * out_data = NULL );

                        /** Start the scanning mode, using parameters stored in the object (loaded from the .ini file)
                          * After this command the device will start to send scans until "switchLaserOff" is called.
                          * \return false on any error
                          */
                        bool  startScanningMode();

                        /** Turns the laser on */
                        void initialize();

                        /** Waits for a response from the device.
                          * \return false on any error
                          */
                        bool  receiveResponse(
                                        const char      *sentCmd_forEchoVerification,
                                        char    &rcv_status0,
                                        char    &rcv_status1,
                                        char    *rcv_data,
                                        int             &rcv_dataLength);


                        /** Assures a minimum number of bytes in the input buffer, reading from the serial port only if required.
                          * \return false if the number of bytes are not available, even after trying to fetch more data from the serial port.
                          */
                        bool assureBufferHasBytes(const size_t nDesiredBytes);

                public:
                        /** Constructor
                          */
                        CHokuyoURG();

                        /** Destructor: turns the laser off */
                        virtual ~CHokuyoURG();

                        /** 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 will be typically called in a different thread than other methods, and will be called in a timely fashion.
                          */
                        void  doProcessSimple(
                                bool                                                    &outThereIsObservation,
                                mrpt::obs::CObservation2DRangeScan      &outObservation,
                                bool                                                    &hardwareError );

                        /** 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.
                          */
                        bool  turnOn();

                        /** 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.
                          */
                        bool  turnOff();

                        /** Empties the RX buffers of the serial port */
                        void purgeBuffers();

                        /** If set to non-empty, the serial port will be attempted to be opened automatically when this class is first used to request data from the laser.  */
                        void setSerialPort(const std::string &port_name) { m_com_port = port_name; }

                        /** Set the ip direction and port to connect using Ethernet communication */
                        void setIPandPort(const std::string &ip, const unsigned int &port) { m_ip_dir = ip; m_port_dir = port; }

                        /** Returns the currently set serial port \sa setSerialPort */
                        const std::string getSerialPort() { return m_com_port; }

                        /** If called (before calling "turnOn"), the field of view of the laser is reduced to the given range (in radians), discarding the rest of measures.
                          *  Call with "0" to disable this reduction again (the default).
                          */
                        void setReducedFOV(const double fov) { m_reduced_fov = fov; }

                        /** Changes the high sensitivity mode (HS) (default: false)
                          * \return false on any error
                          */
                        bool  setHighSensitivityMode(bool enabled);

                        void setVerbose(bool enable = true) { m_verbose = enable; }


                protected:
                        /** Returns true if there is a valid stream bound to the laser scanner, otherwise it first try to open the serial port "m_com_port"
                          */
                        bool  checkCOMisOpen();

                        double                  m_reduced_fov; //!< Used to reduce artificially the interval of scan ranges.

                        std::string             m_com_port;             //!< If set to non-empty, the serial port will be attempted to be opened automatically when this class is first used to request data from the laser.

                        std::string             m_ip_dir;       //!< If set to non-empty and m_port_dir too, the program will try to connect to a Hokuyo using Ethernet communication
                        unsigned int    m_port_dir;     //!< If set to non-empty and m_ip_dir too, the program will try to connect to a Hokuyo using Ethernet communication

                        /** The information gathered when the laser is first open */
                        TSensorInfo             m_sensor_info;

                        bool                    m_I_am_owner_serial_port;

                        uint32_t                m_timeStartUI;  //!< Time of the first data packet, for synchronization purposes.
                        mrpt::system::TTimeStamp        m_timeStartTT;

                        /** See the class documentation at the top for expected parameters */
                        void  loadConfig_sensorSpecific(
                                const mrpt::utils::CConfigFileBase &configSource,
                                const std::string         &iniSection );

                };      // End of class

        } // End of namespace

} // End of namespace

#endif