CSICKTim561Eth_2050101.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/comms/CClientTCPSocket.h>
#include <mrpt/hwdrivers/C2DRangeFinderAbstract.h>

namespace mrpt::hwdrivers
{
class CSICKTim561Eth : public C2DRangeFinderAbstract
{
    DEFINE_GENERIC_SENSOR(CSICKTim561Eth)
   public:
    /** Constructor.
     * Note that there is default arguments, here you can customize IP Adress
     * and TCP Port of your device
     */
    CSICKTim561Eth(
        std::string _ip = std::string("192.168.0.1"),
        unsigned int _port = 2111);
    /** Deconstructor.
     * Close communication with the device, and free memory.
     */
    ~CSICKTim561Eth() override;

    /** This function acquire a laser scan from the device. If an error occured,
     * hardwareError will be set to true.
     * The new laser scan will be stored in the outObservation argument.
     *
     * \exception This method throw exception if the frame received from the
     * TIM 561 contain the following bad parameters:
     * * Status is not OK
     * * Data in the scan aren't DIST1(may be RSSIx or DIST2).
     */
    void doProcessSimple(
        bool& outThereIsObservation,
        mrpt::obs::CObservation2DRangeScan& outObservation,
        bool& hardwareError) override;

    /**
     * This method must be called before trying to get a laser scan.
     */
    bool turnOn() override;

    /**
     * This method could be called manually to stop communication with the
     * device. Method is also called by destructor.
     */
    bool turnOff() override;

    /**
     * This method could be called manually to reboot the device.
     */
    bool rebootDev();

    /**
     * A method to set the sensor pose on the robot.
     * Equivalent to setting the sensor pose via loading it from a config file.
     */
    void setSensorPose(const mrpt::poses::CPose3D& _pose);

    /**
     * This method must be called periodically. Period depend on the
     * process_rate in the configuration file.
     */
    void doProcess() override;

    /** Initialize the sensor according to the parameters previously read
     * in the configuration file.
     */
    void initialize() override;

   private:
    std::string m_ip;
    unsigned int m_port;
    mrpt::comms::CClientTCPSocket m_client;
    bool m_turnedOn{false};
    std::string m_cmd;
    bool m_connected{false};
    unsigned int m_scanFrequency;  // Scanning Hertz
    double m_angleResolution;  // Degrees [0.0001, 1]
    double m_startAngle;  // the minimum startAngle is -45 degree
    double m_stopAngle;  // the maximum stopAngle is 225 degree
    mrpt::poses::CPose3D m_sensorPose;
    double m_maxRange{10.0};  // 10 meter is the range official suggested
    double m_beamApperture;  // the range bewteen m_startAngle to m_stopAngle

    void generateCmd(const char* cmd);  // According to communication protocol
    bool checkIsConnected();  // Check the status about Lidar connection
    bool decodeLogIn(char* msg);
    bool decodeScanCfg(std::istringstream& stream);
    bool decodeScan(
        char* buf, mrpt::obs::CObservation2DRangeScan& outObservation);
    void sendCommand(const char* cmd);
    void roughPrint(char* msg);

   protected:
    /** Load sensor pose on the robot, or keep the default sensor pose.
     */
    void loadConfig_sensorSpecific(
        const mrpt::config::CConfigFileBase& configSource,
        const std::string& iniSection) override;
};
}  // namespace mrpt::hwdrivers